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ABSTRACT 


This thesis provides students with a set of graphics tools allowing them to better 
visualize the effects of gravity-gradient torques on a rigid spacecraft in a low earth orbit. 
It allows the user to select from a variety of rigid bodies of different configurations, place 
them in any orientation at any altitude, apply the appropriate gravity-gradient moments to 
the body and immediately see the effect on the rigid body. This is accomplished through 
interactive computer graphics routines, written to run on Silicon Graphics computers The 
thesis includes a presentation of the theory involved in the programming of the physical 
properties and then discusses the basics of computer graphics including a more detailed 
look at the specific implementation for this thesis A detailed user’s guide i« included to 
train students to use the tools as expeditiously as possible It concludes with 
recommendations for further study \n this area 
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1. INTRODUCTION AND BACKGROUND 


A. INTRODUCTION 

The goal of this thesis is to provide a tool for the visualization of the effects of 
gravity-gradient disturbance torques on a rigid-body spacecraft in an orbit around the 
earth. This is accomplished through the use of three-dimensional computer graphics 
written to emulate the laws of physics and the torques encountered by a spacecraft in a 
typical earth orbit. The system is fully interactive and allows the user to study spacecraft 
bodies of various shapes and sizes, including the Naval Postgraduate School's Petite 
Amateur Navy Satellite (PANSAT). 

This thesis begins with a background discussion of the need for visualization tools 
of this sort It then goes in to a discussion of the physics of gravity-gradient disturbance 
torques as well as the development of the equations for the gravity-gradient moment 
Next it covers the graphical implementation of the program. A discussion of the results 
follows, including an example of the display screen and a tutorial for the user. Finally, a 
chapter on conclusions and recommendations is included. 

B. BACKGROUND 

This thesis was written to provide an educational tool for the analysis of spacecraft 
motion under the influence of the earth's gravity. Presently, there exists several computer 
software programs that can be programmed to simulate the dynamics of a rigid body [Ref. 
1], There are some that are programmed to simulate spacecraft dynamics and control 
[Ref. 2], There are routines that enable the user to study spacecraft orbits and ground 
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tracks [Ref 3] AJ1 of the above routines have one or more disadvantages Some are 
difficult to work with and lack a graphical user's interface [Ref 2] Others give only 
orbital parameters and tell nothing of what is physically happening to the spacecraft [Ref 
3] Some display the spacecraft in a wireframe representation [Ref 3] Still others 
provide analysis in the form of graphs and plots of data [Ref 1], After using some of the 
routines currently available, it became obvious that this was not the optimum way to 
determine what is actually happening to a spacecraft in orbit. Plots of data are helpful and 
can yield useful information, but usually only after in-depth study A more intuitive 
method that would allow a user to see the spacecraft in motion on the display is needed. 
The software described by this thesis was written in an attempt to overcome the 
aforementioned disadvantages and provide that intuitive approach. This program will 
allow the user to view a spacecraft-body under the influence of gravity-gradient torques. 
One can describe the spacecraft body as a block, sphere, cylinder or create a new 
configuration such as PANSAT. The user will be able to specify the body's mass, size and 
dimension, as well as place it at any altitude in any initial orientation. One can then apply 
the initial orbital rotations and the gravity-gradient torques that would be acting on the 
spacecraft. This tool allows the user to experiment with different situations and learn from 
the effects of those situations. There is no better way to learn than by a "hands on" 
approach and this thesis will allow the user to sit at a computer screen and "play" with 
different values and see immediately the effect of those values on his spacecraft. 
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11. DYNAMICS 


A. INTRODUCTION 

This chapter discusses the theory on the dynamics demonstrated in this thesis It 
begins with a discussion of frames of reference, followed by an explanation of Euler's 
equations of motion and finishes with the gravity-gradient moment equation For the sake 
of brevity, the equations used will not be derived here. For a derivation of the stated 
equations, see [Ref. 4,pp 106-112] and [Ref. 5,p. 113], 

B. FRAMES OF REFERENCE 

The motion of a rigid body can be described in several different ways, depending 
upon the frame of reference used As a result, the description of the body's motion is not 
complete without also describing the frame of reference. The two frames of reference 
used in this thesis are the orbit reference frame and the body reference frame The orbit 
frame consists of three orthogonal axes, o t , o 2 and o 3 , that allows one to describe the 
motion of a spacecraft with respect to its orbit plane. For the purposes of this thesis, o, 
will be anti-earth pointing, o 2 will be in the direction of flight and o 3 will be in the direction 
of the orbit normal (see Figure 2.1). The body frame is fixed to the spacecraft's principal 
axes (see Figure 2.2) and coincides with the orbit frame in the absence of spacecraft roll, 
pitch or yaw. If there is a roll, pitch or yaw, these two frames are separated by a rotation 
through that angle about the appropriate axis (see Figure 2.3). One can describe the 
spacecraft's motion in either frame and can switch back and forth between frames by using 
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a Direction Cosine Matrix (DCM) to convert from one set of coordinates to the other A 


complete discussion of DCM's can be found in [Ref. 6] 


o2 



Figure 2.1. Orbit Reference Frame 



Figure 2.2. Body Reference Frame Figure 2.3. Orbit and Body Frames 


4 





C. EULER S EQUATIONS OF MOTION 

Euler's equations of motion are a set of three coupled differential equations that 
describe the effect of applied moments on a rigid body. In the case where the products of 
inertia are zero (principal axes of moment of inertia), Euler’s equations of motion are [Ref 
4,p. 112] 

K = 4 (0 * + (4 - 4 ) ( 31 ) 

M y = I^Wy + G)*CD Z (4 - 4) ( 3 -2) 

M = IJo z + toxtoy (4 - 4 ) ( 3 - 3 ) 


or written as the dynamical equations of motion for a rigid body 

<0, = £ + 


^ i W 1 W 




4x - /wi 


( 3 - 4 ) 
(3.5) 
(3 6 ) 


where 

6)x, (by, and cb z are the rates of change of the angular velocity components 
about the x, y, and z axis, respectively 

(Ox, co y , and co z are the angular velocity components about the x, y, and z axis, 
respectively 

M x , M y and M 2 are the moments about the x, y, and z axis, respectively 

4, 4 , and 4 are the principal moments of inertia about the x, y, and z axis, 
respectively. 
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Furthermore. /„= J n (y 2 +: 2 )dm . 

(3 7) 

L = L (x 2 +z 2 )dm. and 

(3 8) 

L = j m (x 2 + v 2 ) dm 

(3 9) 


with dm being a particle of differential mass and x. and z being the distance from the 
center of mass to that mass particle It should be noted that M is the sum of all external 
moments on the spacecraft such as gravity gradient moment, solar pressure moment and 
control moment, but for the purpose of this thesis, we will only consider the gravity 
gradient moment. 

D. GRAVITY-GRADIENT TORQUES 

A gravity-gradient torque applied to a spacecraft body is due to the difference in 
the distances between the various mass points on the spacecraft body and the center of 
mass of the earth. The magnitude of this torque is dependent upon many factors. These 
factors include the principal moments of inertia, which take into account the moment arm 
of the point mass measured from the center of mass, the altitude of the spacecraft, and the 
orientation of the spacecraft with respect to its orbit frame. The gravity gradient torque 
on an arbitrary spacecraft can be approximated by [Ref. 5,p. 113] 



(Is — Iyy)CnCl3 X 1 
(Ixx ~ f«)CljCi3 y > 

(lyy ~ Ixt)Ci 2 C\iZ J 


(3.10) 


where 

\i e is the earth's gravitational constant and equals 398601.395 km 3 /sec 2 
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A" is the orbit radius and equals the earth s radius 14 kmi plus the 

orbit altitude 

(’ s are the direction cosine matrix (DCM) elements used to express bodv 
coordinates in the orbit frame, where (' = o, • bj 
Substituting the elements of Eq 3 10 into Eqs 3 4. 3 5, and 3 6 the dynamical equations 
of motion including gravity gradient torques become 


CD* = | 

[~r(/= -lyy)C\lC\1 -tOyCOrC/s: ~lyy) ) 

|//« 

(3 11) 

toy = \ 

- /zr)CiiCi3 - C0 z 6)x(/jcc -Izz) ^ 

) ! I yy 

(3.12) 

(0; = | 

^ ~ Ixx)C\lCl\ ~ (Ox(Hy{Iyy ~ /jet) 

)//*= 

(3.13) 


These equations of motion that take into account the gravity gradient torque are used in 
the simulation 
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III. GRAPHICS AND IMPLEMENTATION 


A. INTRODUCTION 

This chapter will discuss how the physics covered in the previous chapter is 
converted from an idea and equations on paper to computerized graphics on the screen It 
will begin with a brief primer on the basics of computer graphics and end with the specifics 
of the implementation of the gravity gradient problem 

B. COMPUTER GRAPHICS 

The programs contained in this thesis were developed and tested on a Silicon 
Graphics Elan computer. Listed below are the minimum hardware and software 
requirements for proper execution of the code. 

Hardware 

• 1 50 MHz IP20 Processor 

• FPU MIPS R4010 Floating Point Processor Chip 

• CPU: MIPS R4000 Processor Chip 

• Data Cache: 8 KB 

• Instruction Cache: 8 KB 

• Secondary Cache: 1 MB 

• Main Memory: 64 MB 

• IRIS Audio Processor: Revision 10 

• Graphics Board: GR2-Elan 

• Mouse 
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Software 


• Operating System Silicon Graphics Irix version 4 05 

• Compiler Silicon Graphics C++ Compiler version 3 0 

It is important to note that a graphics board capable of Z Buffering is essential to the 
correct view of the graphics screens The software will still function without it but the 
displays will be grainy and some objects may appear incomplete It should also be noted 
that the software will nan on any operating system version after 4 05 

The world of computer graphics is complex and a full discussion of the subject is 
beyond the scope of this thesis For a discussion of the basics of computer graphics and 
how they are implemented on Silicon Graphics computers and used by this thesis, see 
Haynes [Ref 7,pp 16-23] 

C. IMPLEMENTATION 

This thesis was conceived as an extension of Haynes [Ref 7], As such it draws 
heavily upon the groundwork previously developed. The goal is to start with a general 
routine and develop it into a specific application to be used by students in their study of 
spacecraft attitude dynamics All coding is done in the programming language C++. 

The software discussed in this thesis is written using several basic tools, some of 
which were previously developed by Haynes. The C++ programming language supports a 
data structure called a class and this data structure is used extensively to define the 
various objects used in the thesis. A class data structure is such that it contains the 
information that defines the data structure as well as all the functions that can operate on 
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the data structure The advantage to this is self-contained, re-usable code is that it is 
resistant to data corruption by routines that are not intended to have access to the data in 
question This thesis it built around three basic classes: the vector3D class, the matrix3x3 
class and the rigid_body class The vector3D class contains a three dimensional vector 
data type used to store such information as a position vector or velocity vector Included 
in this class are several functions used to perform various operations on the vector such as 
vector arithmetic and normalization The matrix3x3 class contains a data type 
representing a 3x3 matrix used to store such information as rotation matrices between two 
reference frames Also included in this class are functions used to perform matrix 
operations such as matrix algebra Finally the rigidbody class contains a data type used 
to represent any rigid body and contains information such as mass, size, location, velocity, 
acceleration or moments of inertia of the rigid body. Most of this information is contained 
in either the vector3D or the matrix3x3 class within the rigidbody class For example, 
velocity is stored in a vector3D class which is in turn stored in the rigid body class and the 
inertia matrix is stored in a matrix3x3 class which is stored in the rigid body class The 
class also contains functions that define the various operations that can be performed on 
the rigid body, such as assigning it a velocity or position, rotating it or changing its size or 
shape For a detailed explanation of these classes and their associated functions, see 
Haynes. 

Another fundamental building block of this thesis is the graphics functions used to 
drive the display The graphics functions are used to build and display the background 
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screen as well as the objects displayed on the background They consist of routines to 
ready the screen for display, routines to change the position from which you are looking 
and the position to which you are looking, routines to display various data fields on the 
background and routines to display various objects on the screen such as rigid bodies or 
more specifically, spacecraft These displayable objects are graphics models that are built 
outside of the software in this thesis and then read into memory for display The 
construction of these models is quite complex and will not be discussed in this thesis For 
a detailed discussion of these graphics models, see Zyda [Ref 8] 

Along with the graphics models themselves, a method of providing motion to them 
is needed Providing motion for the displayed objects is accomplished by the use of 
numerical integrators such as the Runga-Kutta Fourth Order Method and the Runga-Kutta 
Adaptive Step Method (the latter being also know as the Runga-Kutta Fourth/Fifth Order 
Method) These integrators take the current state of the object and the moment applied 
to it to determine the next state 

Finally, a routine is needed to collect and drive all the aforementioned building 
blocks. This is accomplished through a "main" program that determines what action needs 
to take place, when it needs to take place and calls the appropriate routine to make it 
happen. This routine is the "brains" of the program and it uses the classes, graphics, and 
integrators as merely tools to accomplish its mission. 
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IV. USER’S GUIDE 


A. INTRODUCTION 

A software package is useless without the training to use it This chapter will 
provide the user with that training It will present the user with a step-by-step discussion 
of how to use the gravity-gradient visualizer software It contains the visualizer display 
screens including the control buttons that allow the user full control over the simulation 
It also discusses the different procedures required depending on the type of rigid body 
with which the user wishes to work 

B. TUTORIAL 

After logging on the Silicon Graphics Computer, start the gravity-gradient 
visualizer software by typing ggrad at the UNIX prompt It will then take 5-10 seconds 
for the software to load and for the initial graphics screen to be displayed This initial 
screen will consist of a control window and a main display window The initial control 
window and the initial main display window will appear as in Figure 4.1 The initial values 
for inertia, mass, angular velocity and angular momentum are displayed in the main display 
window in addition to the initial rigid body All functions are accessed by clicking on the 
appropriate field once with the left mouse button. It is important to note that mouse clicks 
are valid only if they are performed with the mouse pointer in the control window. 
Clicking in the main display window will have no effect. The only exception to this is that 
the right mouse button can be held down anywhere on the screen to make a selection to 
exit the program The orientation of the main display screen is as follows: 
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Figure 4.1. Display with "Block" Selected. 
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the red axis is anti-earth pointing (o,), the blue axis is along the direction of motion (o : ) 
and the black axis is along the orbit normal (o 3 ) As such, the user is looking down on the 
orbit plane from above with the earth off the left side of the screen (see Figure 2 .1). 

A number of options are available to the user when the initial screen is displayed 
and the selection of these options will be reflected in the displayed rigid body First, the 
user can select the shape of the object he wishes to display, choosing from a sphere, cube 
or cylinder with or without a gravity-gradient boom attached or a model of the PANSAT 
(see Figure 4 1) To make this selection, the user should click on the word for the 
appropriate shape. The procedures for the sphere, cube and cylinder are different than 
those for a sphere, cube and cylinder with a boom, which are different from those for 
PANSAT These three cases will be discussed separately, in a step-by-step fashion 

If the user selects a sphere, cube or cylinder, the procedure would be as follows: 

• Change the mass of the object by clicking the up or down arrows either side of 
the mass field. This will increase or decrease the mass by 50 kilograms for each 
click Changing the mass will result in the moments of inertia being re-calculated 
and redisplayed 

• Change the size of the object by clicking on the up or down arrows either side of 
the size field. This will increase or decrease the size by one meter for each click 
The size of the selected object can be changed along any axis and changing the 
size will result in the moments of inertia being recalculated and re-displayed. 

• Select a rotation angle which represents an initial error in orientation. This is 
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accomplished by clicking the arrows either side of the field marked "Theta", 
increasing or decreasing the angle by five degrees for each click 

• Select an altitude for the rigid body by clicking the arrows either side of the 
altitude field, increasing or decreasing the altitude by 500 kilometers for each 
click. This will cause an initial angular velocity about the z axis to be calculated 
and displayed in the field marked "Ang Vel". 

• Click on the button labeled "Rotate Body" to affect the rotation by the angle 
selected above in the field marked "Theta". 

• Click on the button labeled "Spin Up" to start the rotation about the z axis with 
the angular velocity displayed in the "Ang Vel" field. 

• Click on the button labeled "Gravity Gradient" to affect the gravity gradient 
moment on the rigid body This will result in the torques being calculated from 
the altitude and initial orientation and those being displayed in the field marked 
"Moment". 

• To terminate the simulation and reset the initial values, click on the button 
labeled "Reset". 

At first, the displayed body may not appear to be moving. It is, but very slowly. This 
routine was built to display real-time spacecraft motion. As such, the user views the 
real-time angular motion of the spacecraft. To speed up this displayed motion, an option 
was added to allow the user to increase or decrease the magnitude of these values by a 
factor of ten. This is accomplished with the arrows either side of the fields marked "Vel 
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Mag" and "Mom Mag" Thus, the user can increase the velocity magnitude and the 
moment magnitude in order to better visualize their effects on the spacecraft 

If the user selects a sphere, cube or cylinder with the gravity gradient boom, the 
procedure would be changed slightly The control window and the main display window 
will now appear as in Figure 4.2, assuming that the cube with a boom is selected The 
changes in the procedure are due to the added complexity of calculating the moments of 
inertia for the new rigid body. As a result, if the user changes the mass, the values are 
changed but not the moments of inertia. Additionally, the size field is now changed to the 
inertia field The displayed inertia values are for a standard body of 1000 kilograms, a size 
of one meter in the x, y and z directions and a six meter massless boom with a two 
kilogram tip mass Therefore, instead of changing the mass and size, and then 
re-computing the inertia, tl .2 user is able to change the inertia directly This is 
accomplished by clicking the arrows either side of the "Inertia" field to increment or 
decrement the inertia by five kilogram-meter 2 per click. The remainder of the procedure is 
the same as for the boomless cases. 

The final set of procedures involves PANSAT. The procedures are similar to 
those for the rigid bodies with a boom with some minor exceptions. The similarities are 
once again due to the complexity' in calculating the moments of inertia of PANSAT. The 
differences are due to the need for much greater accuracy in the inertias and the much 
smaller initial attitude enors expected. As a result, the control window and the main 
display window will appear as in Figure 4.3. Note that the changes in the "Theta" field 
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Figure 4.2. Display with "Block" and "Add Boom" Selected. 
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will now be in tenths of a degree increments vice five degree increments and the changes 
in the inertia data field will be incremented by five one-hundredths of kilogram-meter 
instead of one kilogram-meter 2 . In addition to these differences, the changes in altitude 
will be in twenty-five kilometer increments vice 500 kilometer increments. These smaller 
increments will allow the user to make finer adjustments to more closely represent the 
actual PANS AT mission specifics. All other procedures are the same as for the case of 
the boomless rigid bodies. 

In-depth testing has gone into the above procedures with a goal of being user 
friendly in mind Unfortunately, this goal was at times compromised slightly in order 
maintain a user interface consistent with that defined by Haynes [Ref. 7] and to accomplish 
the greater goal of providing an easy to use tool for the desired analysis. With the 
extensive software features and this detailed user's guide usability will not be a problem. 

As a final note, every attempt was made to arrive at values for the data fields that 
are applicable to as many situations as possible. It is recognized that in some cases the 
chosen values for the data fields are not entirely appropriate. It is not recommended that 
the user alter this application in an attempt to tailor it to his specific needs. However, if 
one possesses the requisite knowledge of graphics and programming in C++ to perform 
this a task, such changes are possible. Appendix I contains suggestions to aid in altering 
the preset data fields. 
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V. CONCLUSIONS AND RECOMMENDATIONS 

A. CONCLUSIONS 

The goal of this thesis was to provide a tool for students to use in visualizing the 
effects of gravity-gradient disturbance torques on a rigid-body spacecraft in a typical low 
earth orbit After simulating this same problem with software products that produced data 
plots as results, it is obvious that this thesis provides a better method of analysis. A late, 
but very useful addition to the thesis was the implementation of PANSAT. The Space 
Systems Academic Group (SSAG), charged with oversight of PANSAT, was very 
interested in the final results. The designers of PANSAT were in need of a tool that 
would allow them to leam how PANSAT would behave after release from the Space 
Shuttle They also needed a way to determine how the solar cells on PANSAT would be 
shadowed in various attitudes. This thesis provides them with a tool for this analysis. 
This was an unexpected benefit of the thesis and at the same time validated that 1) this 
type of software was needed and 2) this thesis would have future real-world applications. 
In this respect, the goal was met and this thesis has been successful. 

B. RECOMMENDATIONS 

There are several areas for further study regarding this thesis. First, as mentioned 
earlier, the moment on a rigid body consists of a gravity-gradient moment, a solar pressure 
moment and a control moment. This thesis could be further developed to include the solar 
pressure moment and a control moment in the disturbance torques experienced by the 
spacecraft. In addition to the control torque, a means of damping the spacecraft control 
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could be added Another area for further study could be a feature to allow the user to 
obtain a hard copy of selected data in order to reinforce the graphics display 

A very in-depth follow-on to this thesis would be to convert all the existing code 
from Graphics Library (GL) programming tools to MOTIF programming tools Under 
GL all of the displays must be manually programmed while MOTIF handles the low-level 
programming chores such as constructing pulldown menus and data entry fields. This 
would enable the programmer to spend his time on programming the simulation vice 
programming the basic graphics functions and would add some very useful options such as 
the ability to arbitrarily specify data instead of relying on fixed increments of data fields. 
This would be a major undertaking, as this thesis consists of several thousands of lines of 
code. 
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APPENDIX A. GRAVITY GRADIENT VISUALIZER CODE 

A. CONSTANTS HEADER FILE 

//ifhdef CONST ANTS_H 
#define CONSTANTS H 
#include "vector3D.H" 

const long double pi = 3.145926536, 

const long double deg rad = 0.0174532925; //conversion from deg to rad 

const long double mu earth = 398601.395; // earths gravitational constant 

// (km A 3/sec A 2) 

const long double earth radius = 6378.14; // earth's radius (km) 

#endif 

B. GRAVITY GRADIENT HEADER FILE 

//define BASE H 
//include "constants.H" 

//include "rigidbody.H" 

//include "menu.H" 

C. GRAVITY GRADIENT SOURCE FILE 

//include "base.H" 

#include <stdlib.h> 

#include <iostream.h> 

void main() 

{ 

int section = 0, bypass = 0, NO GO = 0, mx = 0, my = 0, GO = 0, obj = 2, 
go next = 0, axisl = 0, axis2 = 1, axis3 = 2; 

matrix3x3 rotation, matl, mat2, mat3; 

vector3D angular velocity, inertia,size(l 0,10,10), theta(0,0,0), 

pansat_size(l 0,10,10), boom_size(l 0,10,10), reuse, 
omega_vec(0,0,0), initial_omega(0,0,0); 

double mass = 0.0, duration = 99999999.0, elapsed time = 0.0, step = 0.0, 
am, ami, am2, am3, mag = 1.0, total time = 0.0, vel_mag = 1, 
mom mag = 1, rot = 0.0, altitude = 0.0, omega = 0.0, radius = 0.0, 
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x = 0, y = 0, z = 0, mox = 0, moy = 0, moz = 0, 

initialize(); 

jnitialize_menu(), 

init_control_window(); 

main_window(), 

rigidbody cube(l), ball(2), cylinder(3), pansat(60), 

rigidbody ball_boom(71), cube_boom(72), cylinder_boom(73); 

rigid body frame(lOO), axis(200), reuse_body, 

reusebody. assign_shape(cube. retumshapeO); 

reuse_body.assign_mass(cube.retum_mass()), 

reusebody. assign_size(size); 

reusebody.addaxisO; 

reuse body. assign_type( 1); 

reuse_body.compute_inertia(); 

mass = reuse_body.retum_massO; 

set_target(0.0,0.0,00); 

set_ e ye(0.0, 0.0,100); 

set_time(); 

while (section != 99) // while exit not selected 

{ 

section = queue_test(); // what type of interrupt event 

set_delta(); 

view(); 

// draw the controls screen 

gyro_controls(x,y,z,obj,altitude,size,theta,mass,mox,moy,moz, velmag, 
mom mag, elapsed time, inertia); 

if (bypass > 0 && bypass < 6) // system delay for mouse input 

//timing 

bypass++; 

else 

bypass = 0; 

if (section > 99999) // if the event was a mouse 

// selection 

{ 

mx = section /100000; // decode mouse x & y coords 

my = section - (mx * 100000); 
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if (! bypass) 

{ 

bypass = 1; 


if (obj = 7) // PANSAT 

{ 


// Inertia Decrease 

if (mx > 113 && mx < 129) 

{ 

if (my > 936 && my < 953) // Ixx 
if (inertia[0] > 0.05) 

inertia[0] = inertia[0] - 0.05; 

if (my > 923 && my < 937) // Iyy 
if (inertiap] > 0.05) 

inertia[l] = inertiap] - 0.05; 

if (my > 909 && my < 924) //Izz 
if (inertia[2] > 0.05) 

inertia[2] = inertia[2] - 0.05; 

reusebody. assigninertia(inertia); 

} 

// Inertia Increase 

if (mx > 165 && mx < 181) 

{ 

if (my > 936 && my < 953) // Ixx 

inertia[0] = inertia[0] + 0.05; 

if (my > 923 && my < 937) // Iyy 

inertiap] = inertiap] + 0.05; 

if (my > 909 && my < 924) // Izz 

inertia[2] = inertia[2] + 0.05; 

reuse_body. assign Jnertia(inertia); 

} 


else if ((obj >3) && (obj < 7)) // boom object 

{ 
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// Inertia Decrease 

if (mx >113 && mx < 129) 

{ 

if (my > 936 && my < 953) // Ixx 

if (inertia[0] > 1) 

inertia[0] = inertia[0] - 5, 

if (my > 923 && my < 937) // Iyy 

if (inertia[ 1J > 1) 

inertia[l] = inertia[l] - 5; 

if (my > 909 && my < 924) // Izz 

if (inertia[2] > 1) 

inertia[2] = inertia[2] - 5; 

reusebody.assigninertia(inertia); 

} " ~ 

// Inertia Increase 

if (mx > 165 && mx < 181) 

{ 

if (my > 936 && my < 953) // Ixx 

inertia[0] = inertia[0] + 5; 

if (my > 923 && my < 937) // Iyy 

inertia[l] = inertia[l] + 5; 

if (my > 909 && my < 924) // Izz 

inertia[2] = inertia[2] + 5; 

reuse_body. assign_inertia(inertia); 

} 


else // cube, sphere, or cylinder 

{ 


// Size Decrease 

if (mx > 113 && mx < 129) 

{ 

if (my > 936 && my < 953) // size along x axis 

if (size[0] > 1.0) 
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size[0] = size[0] - 1, 


if (my > 923 && my < 937) // size along y axis 

if (size[l] >10) 

size[ 1 ] = size[ 1 ] - 1; 

if (my > 909 && my < 924) // size along z axis 

if (size[2] > 1.0) 

size[2] = size[2] - 1; 

reusebody. assign_size(size); 
reusebody. compute_inertia(); 


// Size Increase 

if (mx > 165 && mx < 181) 

{ 

if (my > 936 && my < 953) // size along x axis 

size[0] = size[0] + 1; 

if(my>923 &&my<937) //size along y axis 

size[l] = size[l] + 1; 

if (my > 909 && my < 924) // size along z axis 

size[2] = size[2] + 1; 

reuse_body.assign_size(size); 

reuse_body.compute_inertia(); 

} 

} 


// Mass Decrease 

if (obj = 7) //PANS AT 

{ 

if ((mx >113 && mx < 129) && (my > 866 && my < 883)) 

{ 

if (mass >31.0) 
mass = mass -1; 

reusebody.assignmass(mass); 
reusebody. computeinertiaO; 

} 
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// all other shapes 


} 

else 

{ 

if ((mx >113 && mx < 129) && (my > 866 && my < 883)) 

{ 

if (mass > 50.0) 
mass = mass - 50; 

reuse_body.assign_mass(mass); 
reusebody. compute_inertia(); 

} 

} 

// Mass Increase 

if (obj — 7) //PANSAT 

{ 

if ((mx > 165 && mx < 181) && (my > 866 && my < 883)) 
i 

mass = mass + 1; 
reusebody.assignmass(mass); 
reusebody. computeinertiaO; 

} 

} 

else 

{ 

if ((mx > 165 && mx < 181) && (my > 866 && my < 883)) 

{ 

mass = mass + 50; 
reusebody. assignmass(mass); 
reusebody. computeinertiaO; 

} 

} 

// Rotation Angle Decrease 
if (mx > 379 && mx < 396) 

{ 

if (my > 936 && my < 953) // angle about x 

{ 

if (obj = 7) 

theta[0] = theta[0] -0.1; 
else 


28 





theta[0] = (int (theta[0]) - 5) % 360, 


> 

if (my > 923 && my < 937) // angle about y 

{ 

if (obj = 7) 

theta[ 1 ] = theta[ 1 ] - 0.1; 
else 

thetafl] = (int (theta[l]> - 5) % 360; 

} 

if (my > 909 && my < 924) // angle about z 

{ 

if (obj — 7) 

theta[2] = theta[2] -0.1; 
else 

theta[2] = (int (theta[2]) - 5) % 360; 

} 

} 


// Rotation Angle Increase 
if (mx > 433 && mx < 448) 

{ 

if (my > 936 && my < 953) // angle about x 

{ 

if (obj = 7) 

theta[0] = theta[0] + 0,1; 
else 

theta[0] = (int (theta[0]) + 5) % 360; 

} 

if(my > 923 && my < 937) // angle about y 

{ 

if (obj = 7) 

theta[ 1 ] = theta[ 1 ] + 0.1; 
else 

theta[l] = (int (theta[l]) + 5) % 360; 

} 

if(my > 909 && my < 924) // angle about z 

{ 
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if (obj — 7) 

theta[2] = theta[2] + 0 1, 

else 

theta[2] = (int (theta[2j) + 5) % 360, 

} 

} 

// Angular Velocity Magnitude Decrease 
if ((mx > 186 && mx < 202) && (my > 866 && my < 883)) 
vel_mag = velmag /10; 

// Angular Velocity Magnitude Increase 
if ((mx > 253 && mx < 269) && (my > 866 && my < 883)) 
vel_mag = vel mag * 10; 

// Moment Magnitude Decrease 

if ((mx > 279 && mx < 296) && (my > 866 && my < 883)) 
mommag = mommag /10; 

// Monment Magnitude Increase 

if ((mx > 346 && mx < 361) && (my > 866 && my < 883)) 
mom mag = mom mag * 10; 

// Altitude Decrease 

if ((mx > 373 && mx < 388) && (my > 866 && my < 883)) 

{ 

if (obj = 7) 

{ 

if (altitude > 0) 

altitude = altitude - 25; 

} ( 

else 

{ 

if (altitude > 0) 

altitude = altitude - 500; 

} 

// compute inertial angular velocity (radians/sec) 
if (altitude != 0) 

{ 

radius = earth_radius + altitude; 

omega = sqrt(mu_earth / (radius*radius*radius)); 

omega_vec[2] = omega;z = omega_vec[2], 
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omega = 0, 

omega_vec[2] = 0,2 = 0, 


else 

} 


} 

} 

// Altitude Increase 

if ((mx > 438 && mx < 456) && (my > 866 && my < 883)) 


if (obj = 7) 

{ 

if (altitude < 99975) 

altitude = altitude + 25, 

} 

else 

{ 

if (altitude < 99500) 

altitude = altitude + 500; 

i 

i 

// compute inertial angular velocity (radians/sec) 

radius = earth radius + altitude; 

omega = sqrt(mu_earth / (radius*radius*radius)); 

omega vec[2] = omega; 

z = omega_vec[2], 


//Select Shape 

if (mx > 19 && mx < 103) 

{ 

if (my > 936 && my < 953) // select ball 

{ 

obj' 1; 

reuse body. assign_shape(baU. retum shapeO); 

reuse_body.assign_type(2); 

reuse_body.assign_size(size); 

reusebody. assign_mass(ball. retum_mass(}), 

reusebody. computeinertiaO; 

mass = reusebody. retum_mass(); 

} 
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if (my > 923 && my < 937) // select cube 


obj = 2, 

reuse body assign_shape(cube.retum_shape()), 

reuse_body.assign_type( 1), 

reusebody assign_size( size); 

reuse_body.assign_mass(cube.retum_mass()), 

reuse_body.compute_inertia(); 

mass = reuse_body.retum_mass(); 

} 

if (my > 909 && my < 924) // select cylinder 

{ 

obj = 3, 

reuse_body.assign_shape(cylinder.retum_shape()), 

reuse_body.assign_type(3); 

reusebody. assignsize(size); 

reusebody. assign_mass(cylinder. retum_mass()); 

reuse body compute inertiaO; 

mass = reuse_body.retum_mass(); 

} 

if (my > 897 && my < 910) // add boom to existing body 

{ 

if (obj =1) // sphere and boom 

{ 

obj = 4; 

reuse_body.assign_shape(ball_boom.retum_shapeO); 
reuse_body.assign_type(71);size = boomsize; 
reuse_body. assign_size(size); 
reuse_body.assign_mass(ball_boom.retum_massO); 
reuse_body.assign_inertia(ball_boom.retum_inertiaO); 
inertia = reuse_body.retum_inertiaO; 
mass = reuse_body.retum_massO; 

} 

if (obj = 2) // cube and boom 

{ 

obj = 5; 

reuse_body.assign_shape(cube_boom.retum_shapeO); 
reuse_body. assign_type(72); 
size = boomsize; 
reuse_body.assign_size(size); 
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reusebody. assign_mass(cube_boom. retum_mass()), 
reuse_body.assign_inertia(cube_boom.retum_inertia()); 
inertia = reuse_body.retum_inertia(), 
mass = reuse_body.retum_mass(); 

> 

if (obj = 3) // cylinder and boom 

{ 

obj = 6; 

reuse body. assign_shape(cylinder_boom retum_shape()); 

reuse_body.assign_type(73) 

size = boom_size; 

reuse_body.assign_size(size); 

reuse_body.assign_mass(cylinder_boom.retum_mass()); 
reuse_body.assign_inertia(cylinder_boom.retum_inertia()); 
inertia = reuse_body.retum_inertia(); 
mass = reuse_body.retum_mass(); 

} 

} 

if (my > 884 && my < 898) // select PANSAT 

{ 

obj * 7; 

reuse_body.assign_shape(pansat.retum_shapeO); 

reuse_body.assign_type(60); 

size = pansatsize; 

reusebody. assign_size(size); 

reusebody. assign_mass(pansat. retummassO); 

reuse_body.assign_inertia(pansat.retum_inertiaO); 

inertia = reuse_body.retum_inertia(); 

mass = reuse_body.retum_mass(); 

theta.zeroO; 

altitude = 0; 

} 


// Rotate Body button selected 
if (mx > 640 && mx < 707 && my > 890 && 
my < 959 && !NO_GO) 

{ 

// compute the DCM 

matl .DCM_x_rotation(theta[0] * (pi /180)); 
mat2.DCM_y_rotation(theta[l] * (pi /180)); 
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mat3.DCM_z_rotation(theta[2] * (pi / 180)); 
rotation = rotation * mat3; 
rotation = rotation * r.iat2; 
rotation = rotation * mat 1; 

// initial spin is a function of the inertial angular velocity 
// about the 3 axes 

initial_omega = rotation * omega_vec; 
x = initial_omega[0]; 
y = initial_omega[l]; 
z = initial_omega[2]; 

GO = 21; 
step = 0.0; 


// Inertial Moment button selected 
if (mx > 740 && mx < 813 && my > 890 && 
my < 959 && !NO_GO) 

{ 

GO= 11; 

elapsedtime = 0.0; 
step = 0.0; 


// Spin Up button selected 
if (mx > 840 && mx < 907 && my > 890 && 
my < 959 && !NO_GO) 

{ 

GO = 1; 

NOGO = 1; 


if (GO = 2) 
NOGO = 0; 


// RESET button selected 

if (mx > 940 && mx < 1007 && my > 890 && my < 959) 

{ 

reusebody.zeroO; 
reuse_body. assign_size(size); 
mass = reusebody.retummassO; 
inertia = reusebody.retuminertiaO; 
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x = 0; 
y = 0; 

Z = 0; 
mox = 0, 
moy = 0, 
moz = 0, 
velmag =1.0; 
mommag = 1.0; 
theta. zero(); 

GO = 0; 

NOGO = 0; 
step = 0.0; 
elapsedtime = 0.0; 
altitude = 0; 
omega = 0; 
omega_vec.zero(); 
initialomega. zeroO; 
matl.reset(); 
mat2.reset0; 
mat3.reset(); 
rotation. resetO; 

} 

} 

} 

main_window(); 

// this section rotates the body from its inertial frame 

if (GO = 21) // reparation for 1st rotation 

{ 

reuse = reuse * 0; 
if(theta[0]) 

reuse[axisl] = .3 * (theta[0] / fabs(theta[0J)); 

// set ang velocity 

rot = theta[0] * (pi /180) / 2; 

GO++; 
gonext = 0; 


if(GO = 22) // animates body 

{ 

if(go_next) // stop body for second rotation 
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{ 

GO++; 

reuse_body.assign_ang_velocity_bc(0,0 ( 0); 

} 

else 

{ 

if((theta[0] > 0 && rot > .3 * read_deltaO) || (theta[0] < 0 && 
rot < -.3 * read_delta())) 

{ // rotation incomplete 

reuse_body.assign_ang_velocity_bc(reuse); 
rot = rot - reuse[axisl] * read_delta(); 

} 

else // finish 1st rotation 

{ 

if(theta[0]) 

reuse = reuse * (rot / (reuse[axisl] * read_delta())); 
reuse_body.assign_ang_velocity_bc(reuse); 
gonext = 1; 

} 

} 

} 

if(GO = 23) // prep for 2nd rotation 

{ 

reuse = reuse * 0; 
if(theta[l]) 

reuse[axis2] = 3 * (theta[l] / fabs(theta[l])); 
rot = theta[l] * (pi / 180)/ 2 ; 

GO++; 
gonext = 0; 

} 

if(GO = 24) // animate 2nd rotation 

{ 

if(go_next) // stop for 3rd rotation 

{ 

GO++; 

reuse_body.assign_ang_velocity_bc( 0 , 0 , 0 ); 

} 

else 
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{ 

if((theta[l] > 0 && ret > .3 * read_delta()) || (theta[ 1 ] < 0 && 
rot < -.3 * read_delta())) 

{ 

reuse_body.assign_ang_velocity_bc(reuse), 
rot = rot - reuse[axis2] * read_delta(); 

> 

else // finish 2nd rotation 

{ 

if(theta[l]) 

reuse = reuse * (rot / (reuse[axis2] * read_delta())); 
reuse_body. assign_ang_velocity_bc(reuse); 
go_next = 1; 

} 

} 

} 

if(GO = 25) //prep for 3rd rotation 

{ 

reuse = reuse * 0; 
if(theta[2]) 

reuse[axis3] * .3 * (theta[2J / fabs(theta[2])); 
rot = theta[2] * (pi / 180) / 2; 

GO++; 
gonext = 0; 

} 

if(GO = 26) // animate 3rd rotation 

{ 

if(go_ n ext) 

{ 

reuse_body.assign_ang_velocity_bc(0,0,0); 

GO++; 

} 

else 

{ 

if((theta[2] > 0 && rot > .3 * read deltaO) II (theta[2] < 0 && 
rot < -.3 * read_deltaO)) 

{ 

reusebody. assign_ang_velocity_bc(reuse); 
rot = rot - reuse[axis3] * read_deltaO; 
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else 


// finish 3rd rotation 


i 

if(theta[2]) 

reuse = reuse * (rot / (reuse[axis3] * read_delta())); 
reuse_body.assign_ang_velocity_bc(reuse); 
gonext = 1; 

} 

} 

} 

if (GO >11) //for rotations only 

{ 

reuse_body.update_state_rk4(); 
reusebody. displayO; 

} 


if (GO = 1) // spin body 

{ 

reuse_body.assign_ang_velocity_bc(x * velmag, y * velmag, 

z * vel mag); 

GO++; 

} 

if (GO =11) //set moment in ine? dal coordinates 

{ 

mox = 3 * omega * omega * ((reuse_body.retum_inertia())[2]- 

(reuse body.return_inertia())[l]) * rotation[l] * rotation[2]; 

moy = 3 * omega * omega * ((reuse_body.retum_inertiaO)[0]- 

(reuse_body.retum_inertia0)[2]) * rotation[0] * rotation[2]; 

moz = 3 * omega * omega * ((reuse_body.retum_inertiaO)[l]- 

(reuse_body.retum_inertiaO)[0]) * rotation[l] * rotation[0]; 

reuse_body.assign_moment(mox * mom mag, moy * mom mag, 

moz * mom mag); 


elapsed_time = elapsedtime + step; 

} 






// apply moment 


if ((GO ==11) && duration < 0) 

{ 

GO = 0 ; 

NOGO = 0; 

} 

frame display(), 

if (duration > 0 && duration < step) // last integration step 

setdelta(duration), 

step = reuse body update_state_rk45(.000001), 
reusebody.displayO, 

angularvelocity = reuse_body.retum_ang_velocity_bc(); 
inertia = reuse body retum inertiaQ; 

ami = angular_velocity[0] * inertia[0]; 
am2 = angular_velocity[l] * inertia[l]; 
am3 = angular_velocity[2] * inertia[2]; 
am = sqrt(aml * ami + am2 * am2 + am3 * am3); 

// display statistics 

stat_controls(angular_velocity[0],angular_velocity[l],angular_velocity[2], 
am,reuse_body.return_mass(), (reuse_body.retum_inertiaO)[0], 
(reuse_body.retum_inertiaO)[ 1 ], (reusebody. retuminertiaQ) [2]), 
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APPENDIX B. GRAPHICS CODE 


A. HEADER FILE 

#ifndef GRAPHICSH 
#define GRAPHICS_H 
#include <math h> 

#include "vector3D.H" 

#include "matrix3x3.H" 

//include "quaternion.H" 

//include "rdobj opcodes h" 

#include "rdobj funcs.h" 

//include <stdio.h> 

//include <gl h> 

#include <device h> 

//initializes the graphic system 
void initialize(); 

//initializes control window 
void init control_window(); 

//make viewing window active 
void main windowO; 

// makes control window active 
void control_window(); 

//clears control window 
void clear_control_window(), 

//control window for euler program 

void euler_controls(int, int, int, int, int, int,int,quaternion, double); 

//control window for gyro program 

void gyro_controls(double, double, double, int,double, vector3D, vector3D, double, 

double, double, double,double, double, double, vector3D); 

//statisic display for gyro program 

void stat_controls(double, double, double, double,double, double, double, double); 
//control window for frame program 

void frame_controls(int, int, vector3D, vector3D,vector3D, int); 
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//standard function for viewing a scene 
void view(), 

//used to view the scene for a point of view fixed to a rigid body 
void view(quatemion, vector3D, int), 

//attaches the eye to a rigid body 
void attach_eye_to(vector3D*, int*); 

//attaches the tatget to a rigid body 
void attach_target_to(vector3D*); 

//attaches the eye to a rigid body 
void set_eye_to(double, double, double); 

//attaches the target to a rigid body 
void set_target_to(double, double, double); 

//rotates the view in tenths of degrees 
void rotate view(int); 

//displays the body axes of a rigid_body 
void view axisQ; 

//gravity check - returns non zero value when gravity is turned on 

int gravity statusO, 

void set_gravity_on(); 

void set^gravity offO; 

void togglejgravityO; 

//air resistance check - returns non zero value when air resistance is turned on 

int airresistancestatusO; 

void set_air_resistance_on(); 

void set_air_resistance_off0; 

void toggle_air_resistance(); 

// c routines the must be accessed 
extern "C" 

{ 

extern OBJECT* read_object(char[]); 

extern void ready_object_for_display( OBJECT* ); 

extern void display_this_object( OBJECT* ); 
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#endif 


B. SOURCE FILE 

#ifhdef GRAPHIC S C 
#define GRAPHICS_C 
//include "graphics.H" 

#define NEARDEPTH 0x000000 /* the near and far planes used for Zbuffering*/ 

#define FARDEPTH 0x7fffif 

OBJECT *lightobj, *axis, 

//eye and target are the global variables that control the view point 
//and reference point of the scene respectively 

vector3D *eye = new vector3D(10.0, 10.0, 10.0), * target = new vector3D; 

int *eye_display field = NULL; 

int gravityflag = 0, airresistanceflag = 0; 

int twist = 0; 

long main win, control win; 

Matrix un = { 1.0, 0.0, 0.0, 0.0, 

0 . 0 , 1 . 0 , 0 . 0 , 0 . 0 , 

0 . 0 , 0 . 0 , 1 . 0 , 0 . 0 , 

0 . 0 , 0 . 0 , 0 . 0 , 1 . 0 }; 


int gravitystatusO 

{ 

return gravity flag; 

} 


void set jgravity onO 

{ 

gravityflag = 1; 

} 


void set_gravity_oS() 

{ 
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gravity_flag = 0, 

} 


void toggle_gravity() 

{ 

if(gravity flag) 

{ 

gravity_flag = 0; 

} 

else 

{ 

gravityflag = 1; 

} 

} 


int air resistance statusO 

{ 

return airresistanceflag; 

} 


void set air resistance onO 

{ 

airresistanceflag = 1; 

} 


void set air resistance offO 

< 

airresistanceflag = 0, 

} 


void toggleairresistanceO 

{ 

if (air resistance flag) 

{ 

airresistanceflag = 0; 

} 

else 

{ 
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air resistanceflag = 1, 


void initialize() 

{ 

/* set up the preferred aspect ratio */ 
keepaspect(XMAXSCREEN+1, YMAXSCREEN+1); 
prefsize(XMAXSCREEN/2,YMAXSCREEN/2), 
prefposition(0,XMAXSCREEN * 0.8 ,0,YMAXSCREEN * 0.8); 

/* open a window for the program */ 
main win = winopen("Main"); 

vvintitle(’'Gravity-Gradient Visualizer, By Jeff Stewart"); 

/* put the IRIS into double buffer mode */ 
doublebufferO, 

/* put the iris into rgb mode */ 

RGBmode(), 

/* configure the IRIS (means use the above command settings) */ 
gconfig(), 

/* set the depth for z-bufifering */ 
lsetdepth(NEARDEPTH,FARDEPTH); 

/* queue the redraw device */ 
qdevice(REDRAW); 

/*queue the menu button*/ 
qdevice(MENUBUTTON); 

/*tum the cursor on*/ 
cursonO; 

/*select gouraud shading*/ 
shademodel(GOURAUD); 

/*tum on the new projection matrix mode*/ 
mmode(MVIEWING); 
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/Turn on Zbuffering*/ 
zbufFer(TRUE); 

lightobj = read_object("the_light off'), 
axis = read_object("axis off'); 
readyobjectfordisplay(lightobj); 
ready object for display(axis); 

//queue up input devices 

qdevice(LEFTMOUSE); 

qdevice(RIGHTMOUSE); 

qdevice(MOUSEX); 

qdevice(MOUSEY); 

qdevice(RIGHT ARROWKE Y), 

qdevice(LEFTARROWKEY); 

qdevice(UP ARROWKE Y); 

qdevice(DOWNARROWKEY); 

qdevice(SPACEKEY); 

qdevice(EQUALKEY); 

qdevice(MINUSKEY); 

qdevice(FlKEY); 

qdevice(F2KEY); 

qdevice(F3KEY), 

qdevice(F4KEY), 

qdevice(F5KEY); 

qdevice(F6KEY); 

qdevice(F7KEY); 

qdevice(F8KEY); 

qdevice(F9KEY); 

qdevice(F 1OKE Y); 

qdevice(Fl 1KEY); 

qdevice(F12KEY); 

//clear draw and display buffer 

czclear(0xFFFF7200,getgdesc(GD_ZMAX)); 

swapbuffersQ; 

czclear(0xFFFF7200,getgdesc(GD_ZMAX)); 


void init control windowO 

{ 

/* set up the preferred aspect ratio */ 

prefposition(0,XMAXSCREEN * 0.8,YMAXSCREEN * 0.87, YMAXSCREEN); 
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/* open a window for the program */ 
controlwin = winopen("control"), 
wintitle("System Control Window"), 

/* put the IRIS into double buffer mode */ 
doublebuffer(); 

RGBmode(); 

gconfig(); 

pushmatrix(); 

ortho2(0.0, 769.0, 0.0, 100.0), 

RGBcolor(255,255,255); 

clear(), 

popmatrix(); 

swapbuffers(); 

} 


void main_window() 

{ 

winset(mainwin); 

} 


void control windowO 

{ 

winset(controlwin); 

} 


void clear control windowQ 

{ 

winset(controlwin); 

pushmatrixO; 

ortho2(0.0, 769.0, 0.0, 100.0); 

RGBcolor(255,255,255); 

clearO; 

popmatrix(); 

swapbuffers(); 

winset(mainwin); 

} 


47 






void gyro_controls(double x, double y, double z, int object, double altitude, vector3D 
size, vector3D theta, double mass, double tl, double t2, double 

t3, double vel mag, double mommag, double elapsed, 

vector3D inertia) 


{ 

float pt [3][2] = { {142,48}, 
{148,48}, 
{145,42}}; 
on 

float pt2 [3][2] = { {182,42}, 
{188,42}, 
{185,48}}; 


// down arrow starting coordinates, begins 
// Z velocity 


// up arrow starting coodinates, begins on Z 
// velocity 


char s[32]; 

winset(control_win); 

pushmatrix(); 

ortho2(0.0, 769.0, 0.0, 100.0); 

RGBcolor(255,255,255); 

clear(); 

//Go & Reset Buttons 

RGBcolor(200,200,200); 

rectf(475.0, 25.0, 525.0, 75.0); 

rectf(550.0, 25.0, 605.0, 75.0); 

rectf(625.0, 25.0, 675.0, 75.0); 

rectf(700.0, 25.0, 750.0, 75.0); 

RGBcolor(0,0,0); 

cmov2(482, 50); 

charstr("Rotate"); 

cmov2(487, 40); 

charstr("Body"); 

cmov2(555, 50); 

charstr("Gravity"); 

cmov2(552, 40); 

charstr("Gradient"); 

cmov2(639,50); 

charstr("Spin"); 

cmov2(643,40); 

charstr("up"); 

cmov2(710,45); 

charstr("Reset"); 
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// Angular Velocity 

RGBcolor(0,0,255), 

rectf( 140.0, 40.0, 190.0, 70.0), 

RGBcoIor(255,255,0); 

cmov2(142, 61); 

sprintf(s, "%.4f', (double) x), 

charstr(s), 

cmov2(142, 51); 

sprintf(s, "%.4f\ (double) y), 

charstr(s); 

cmov2(142, 41); 

sprintf(s, "%.4f', (double) z); 

charstr(s); 

// External Moment 

RGBcolor(0,0,255); 

rectf(210.0, 40.0, 260.0, 70.0); 

RGBcolor(255,255,0); 

cmov2(212, 61); 

sprintf(s, "%.4f', tl); 

charstr(s); 

cmov2(212, 51); 

sprintf(s, "%.4f', t2); 

charstr(s), 

cmov2(212, 41); 

sprintf(s, "%.4f', t3); 

charstr(s); 

// Rotation angle. Theta 
RGBcolor(0,0,255); 
rectf(290.0, 40.0, 320.0, 70.0); 
RGBcolor(200,200,200); 
rectf(280.0 ) 40.0, 290.0, 70.0); 
rectf(320.0, 40.0, 330.0, 70.0); 
// Draw up and down arrows 
RGBcolor(0,0,0); 
pt[0][0] = pt[0][0] + 140.0; 
Pt[l][0] = pt[l][0]+ 140.0; 
pt[2][0J = pt[2][0] +140.0; 
pt2[0][0] = pt2[0][0] + 140.0; 
pt2[l][0] = pt2[l][0] + 140.0; 
pt2[2][0] = pt2[2][0] + 140.0; 
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polf2(3,pt),polf2(3,pt2); 
pt[0][l] = pt[0][l]+10.0, 
pt[l][l] = pt[l][l] + 10.0; 

P t[2][l] = pt[2][l] + 10.0; 

Pt2[0][l] = pt2[0][l] + 10.0; 

P t2[ 1 ][ 1 ] = P t2[ 1 ][ 1 ] + 10.0; 

P t2[2][l] = pt2[2][l] + 10.0; 
polf2(3,pt);polf2(3,pt2); 
pt[0][l] = pt[0][l]+ 10.0; 
pt[l][l]-pt[l][l]+10.0; 

P t[2][l] = pt[2][l] + 10.0; 

P t2[0][l] = pt2[0][l] + 10.0; 

P t2[l][l] = pt2[l][l] + 10.0; 

P t2[2][l] = pt2[2][l] + 10.0; 

polf2(3,pt); 

polf2(3,pt2); 

RGBcolor(255,255,0); 

if (object = 7) // PANS AT 

{ 

cmov2(292, 61); 
sprintf(s,"%. If, theta[0]); 
charstr(s); 
cmov2(292, 51); 
sprintf(s, "%.lf, theta[l]); 
charstr(s); 
cmov2(292, 41); 
sprintf(s, "%.lf, theta[2]); 
charstr(s); 

} 

else // all other shapes 

{ 

cmov2(292, 61); 
sprintf(s, "%.0f', theta[0]); 
charstr(s); 
cmov2(292, 51); 
springs, ”%.0f', theta[l]); 
charstr(s); 
cmov2(292, 41); 
sprintf(s, "%.0f\ theta[2]); 
charstr(s); 

} 
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//Clock 

RGBcoIor(255,0,0); 
rectfI355 0, 55.0, 460 0, 85 0); 

RGBcolor(0,0,0), 
cmov2(365 0,71 0), 
charstr(''Elapsed Time"); 

RGBcolor(255,255,255), 
cmov2(390, 60); 
sprintf^s,"%. If, elapsed); 
charstr(s), 

RGBcolor(0,0,0), 
cmov2(10 0,90 0); 

charstr("Gravity Gradient Control Window"), 

cmov2( 10.0,75 0), 

charstr("Shape"), 

if (object > 3) // PANSAT or boom object 

{ 

cmov2(82.0,75.0), 
charstr("Inertia"), 

} 

e ' se // cube, sphere, or cylinder 

{ 

cmov2(92.0,75.0); 
charstr("Size"), 

} 

cmov2(143.0,75.0), 
charstr("Ang Vel"); 

RGBcolor(255,0,0), 
cmov2( 197.0,61.5); 
charstr("X"); 

RGBcolor(0,0,255); 
cmov2( 197.0,51.5); 
charstr("Y"); 

RGBcolor(0,0,0); 
cmov2( 197.0,41.5); 
charstr("Z"); 
cmov2(215.0,75.0); 
charstr( "Moment"); 
cmov2(288.0,75.0); 
charstr("Theta"); 
cmov2(92.0,23.0); 
charstr("Mass"); 
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cmov2( 143.0,23 0), 
charstr("Vel Mag"), 
cmov2(211.0,23.0), 
charstr("Mom Mag"), 
cmov2(280.0,23.0); 
charstr("Altitude"); 

// Select between the different base objects 

RGBcolor(0,0,255); 

rectal0.0, 20.0, 70 0, 70.0); 

RGBcolor(255,255,0); 

cmov2( 15.0,61.5); 

charstr("Sphere"), 

cmov2(l 5.0,51.5); 

charstr("Block"); 

cmov2(15.0,41.5); 

charstr("Cylinder"); 

cmov2(15.0,31.5); 

charstr("Add Boom"); 

cmov2(15.0,21.5); 

charstr("PANSAT"); 

switch(object) 

{ 

case 1: 

RGBcolor(255,255,0); 
rectf(10.0, 60.0, 70.0, 70.0); 
RGBcolor(0,0,255); 
cmov2(15.0,61.5); 
charstr(" Sphere"); 
break; 


case 2; 

RGBcolor(255,255,0); 
rectf(10.0, 50.0, 70.0, 60.0); 
RGBcolor(0,0,255); 
cmov2(15.0,51.5); 
charstr("Block"); 
break; 

case 3: 

RGBcolor(255,255,0); 
rectf(10.0, 40.0, 70.0, 50.0); 
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RGBco!or(0,0,255), 
cmov2( 15 0,41.5), 
charstr( "Cylinder"), 
break. 


case 4 

RGBcolor(255,255,0), 
rectal0 0, 60 0, 70 0, 70 0), 
rectal0 0, 30 0, 70 0, 40 0), 
RGBcolor(0,0,255), 
cmov2(15 0,61 5); 
charstr("Sphere"); 
cmov2(15 0,31 5); 
charstr("Add Boom"), 
break. 


case 5: 

RGBcolor(255,255,0); 
rectal0.0, 50.0, 70 0, 60.0); 
rectal 0.0, 30.0, 70.0, 40.0); 
RGBcolor(0,0 255); 
cmov2( 15 0,51.5); 
charstr("Block"), 
cmov2( 15.0,31.5), 
charstr("Add Boom"); 
break; 


case 6: 

RGBcolor(255,255,0); 
rectf(10.0, 40.0, 70.0, 50.0); 
rectf(10.0, 30.0, 70.0, 40.0); 
RGBcolor(0,0,255); 
cmov2(15.0,41.5); 
charstr("Cylinder"), 
cmov2(15.0,31.5); 
charstr("Add Boom"); 
break; 


case 7: 

RGBcolor(255,255,0), 
rectf(10.0, 20.0, 70.0, 30.0); 
RGBcolor(0,0,255); 
cmov2(l 5.0,21 5); 
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charstr(" PAN S AT"); 
break. 


default: 

RGBcolor(255,255,0), 

rectf(10.0, 50.0, 70.0, 60.0); 

RGBcolor(0,0,255); 

cmov2(15.0,51.5); 

charstr("Block"); 

break; 

} 

// Object Size 
RGBcolor(0,0,255); 
rectf(90.0, 40.0, 120.0, 70.0); 
RGBcolor(200,200,200); 
rectf(80.0, 40.0, 90.0, 70.0); 
rectal20.0, 40.0, 130.0, 70.0); 

// Draw up and down arrows 
RGBcolor(0,0,0); 

pt[0][0) = pt[0][0] - 200.0; 
pt[l][0] = pt[l][0]- 200.0; 
pt[2][0] = pt[2][0] - 200.0; 
pt2[0][0] = pt2[0][0] - 200.0; 
pt2[l][0] = pt2[l][0] - 200.0; 
pt2[2][0] = pt2[2][0] - 200.0; 

polf2(3,pt); 

polf2(3,pt2); 

pt[0][l] = pt[0][l]- 10.0; 
pt[l][l] = pt[l][l]-10.0; 

P t[2][l] = pt[2][l] - 10.0; 

Pt2[0][l] = pt2[0][l] - 10.0; 
pt2[l][l] = pt2[l][l] - 10.0; 

P t2[2j[l] = pt2[2][l] - 10.0; 
polf2(3,pt); polf2(3,pt2); 
pt[0][l] = pt[0][l] - 10.0; 
pt[l][l] = pt[l][l] - 10.0; 
pt[2][l) = pt[2][l] - 10.0; 
pt2[0][l] = pt2[0][l] - 10.0; 

P t2[l][l] = pt2[l][l] - 10.0; 

P t2[2][l] = pt2[2][l] - 10.0; 
polf2(3,pt); polf2(3,pt2); 
RGBcolor(255,255,0); 


54 








if (object = 7) // Inertia for PANSAT 

{ 

RGBcolor(255,255,0), 

cmov2(92, 61); 

sprintf(s, "%.2f\ inertia[0]), 

charstr(s), 

cmov2(92, 51), 

sprintf(s, "%.2P, inertia[ 1 ]), 

charstr(s); 

cmov2(92, 41); 

sprintf(s, "%.2f', inertia[2]); 

charstr(s); 

} 

else if ((object >3) && (object < 7)) // Inertia for boom object 

{ 

RGBcolor(255,255,0); 

cmov2(92, 61), 

sprintf(s, "%.0f', inertia[0]); 

charstr(s); 

cmov2(92, 51), 

sprintf(s, "%.0f', inertia[l]); 

charstr(s); 

cmov2(92, 41); 

sprintf(s, "%.0f\ inertia[2]); 

charstr(s); 

} 

else II size for cube, sphere, or cylinder 

{ 

cmov2(92, 61); 
sprintf(s, "%.0f', size[0]); 
charstr(s); 
cmov2(92, 51); 
sprintf(s, "%.0f', size[l]); 
charstr(s); 
cmov2(92, 41); 
sprintf(s, H %.0f, size[2]); 
charstr(s); 

} 

//Object Mass 

RGBcolor(0,0,255); 

rectf(90.0, 8.0, 120.0, 18.0); 







RGBcolor(200,200,200), 
rectf(80.0, 8 0, 90.0, 18 0); 
rectf{120 0, 8 0, 130.0, 18 0); 
// Draw up and down arrows 
RGBcolor(0,0,0); 

Pt[0][l] = pt[0][l] - 32.0; 

Pt[ 1 ][ 13 = Pt[ 1 ][ 1 ] - 32.0; 
Pt[2][l] = pt[2][l] - 32.0; 
P t2[0][l] = pt2[0][l] - 32.0, 
P t2[l][l] = pt2[l][l] - 32.0; 
P t2[2][l] = pt2[2][l] - 32.0; 
polf2(3,pt); polf2(3,pt2); 
RGBcolor(255,255,0); 
cmov2(92, 10), 
sprintf(s, "%.0f', mass), 
charstr(s), 

// Angular Velocity Magnitude 
RGBcolor(0,0,255); 
rectf( 145.0, 8.0, 185.0, 18.0); 
RGBcolor(200,200,200); 
rectf(135.0, 8.0, 145.0, 18.0); 
rectf( 185.0, 8.0, 195.0, 18.0); 
// Draw up and down arrows 
RGBcolor(0,0,0); 
pt[0][0] = pt[0][0] + 55.0; 
Pt[l][0] = pt[l][0] + 55.0; 
Pt[2][0] = pt[2][0] + 55.0; 
pt2[0][0] = pt2[0][0] + 65.0; 
P t2[l][0] = pt2[l][0] + 65.0; 
pt2[2][0] = pt2[2][0] + 65.0; 
polf2(3,pt); 
polf2(3,pt2); 
RGBcolor(255,255,0); 
cmov2(147, 9), 
springs, "%.0e", vel_mag); 
charstr(s); 

// External Moment Magnitude 
RGBcolor(0,0,255); 
rectf(215.0, 8.0, 255.0, 18.0); 
RGBcolor(200,200,200); 
rectf(205.0, 8.0, 215.0, 18.0); 
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rectf(255 0, 8 0, 265 0, 18.0); 
// Draw up and down arrows 
RGBcolor(0,0,0), 
pt[0][0] = pt[0][0] + 70.0; 
Pt[l][0] = pt[l][0] + 70.0; 
Pt[2][0] = pt[2][0] + 70.0, 
pt2[0][0] = pt2[0][0] + 70.0, 
P t2[l][0] = pt2[l][0] + 70.0; 
pt2[2][0] = pt2[2][0] + 70.0; 
polf2(3,pt); 
polf2(3,pt2); 

RGBcolor(255,255,0); 
cmov2(217, 9); 
sprintf(s, "%.0e", mommag); 
charstr(s); 

// Object altitude 
RGBcolor(0,0,255); 
rectf(285.0, 8.0, 325.0, 18.0); 
RGBcolor(200,200,200); 
rectf(275.0, 8.0, 285.0, 18.0); 
rectf(325.0, 8.0, 335.0, 18.0); 
// Draw up and down arrows 
RGBcolor(0,0,0); 

Pt[0][0] = pt[0][0] + 70.0; 
Pt[l][0] = pt[l][0] + 70.0; 
pt[2][0] = pt[2][0] + 70.0; 
pt2[0][0] = pt2[0][0] + 70.0, 
Pt2[l][0] = pt2[l][0] + 70.0; 
pt2[2][0] = pt2[2][0] + 70.0; 
polf2(3,pt); 
polf2(3,pt2); 
RGBcolor(255,255,0); 
cmov2(287, 9); 
sprintf(s, "%.0f', altitude); 
charstr(s); 

popmatrixO; 

swapbufFersQ, 


void stat_controls(double x, double y, double z, double am, double mass, double lx, 

double Iy, double Iz) 
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winset(mainwin), 
char s[32], 
RGBcolor(0,0,0), 
RGBcolor(255,0,0), 
cmovs(25, 35, 0), 
charstr("X velocity"); 
cmovs(37, 35, 0), 
sprintf^s, "%.5f, x), 
charstr(s); 
cmovs(-50, 35, 0); 
charstr("Ixx"); 
cmovs(-45, 35, 0); 
sprintf(s, ”%.5f', lx); 
charstr(s); 

RGBcolor(0,0,255); 
cmovs(25, 33, 0); 
charstr("Y velocity"); 
cmovs(37, 33, 0); 
sprintf(s, ”%.5f’, y); 
charstr(s), 
cmovs(-50, 33, 0); 
charstr("Iyy"); 
cmovs(-45, 33, 0); 
sprintf(s, "%.5f', Iy); 
charstr(s); 
RGBcolor(0,0,0); 
cmovs(25, 31, 0); 
charstr("Z velocity"); 
cmovs(37, 31, 0); 
sprintf(s, "%.5f', z); 
eharstr(s); 
cmovs(-50, 31, 0); 
charstr("Izz"); 
cmovs(-45, 31, 0); 
sprintf(s, "%.5f', Iz); 
charstr(s); 
cmovs(25, 27, 0); 
charstr("Ang Momentum"); 
cmovs(39, 27, 0); 
springs, "%.3f', am); 
charstr(s); 
cmovs(-50, 27, 0); 







charstr("Mass"), 
cmovs(-45, 27, 0), 
sprintf(s, "% 3f’ mass); 
charstr(s), 

I 


void attach eye to(vector3D* v, int* i) 

{ 

if (eye_display_field != NULL) 

{ 

*eye_display_field = 1; 

} 

eye = v; 

eye display field = i; 
*eye_display_field = 0, 
twist = 0; 

} 


void attach target to(vector3D* v) 

{ 

target = v; 
twist = 0; 

> 


void set_eye_to(double x, double y, double z) 

{ 

if (eye display field != NULL) 

{ .. 

*eye_display field = 1; 
eyedisplayfield = NULL; 

} 

eye = new vector3D(x, y, z); 
twist = 0; 

} 


void set_target_to(double x, double y, double z) 

{ 

target = new vector3D(x, y, z); 
twist = 0; 
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} 


void view() 

{ 

swapbufFers(); 

czclear(0xFFFF7200,getgdesc(GD_ZMAX)); 

loadmatrix(un), 

perspective(450,1.25,0.2,10000.0); 

lookat((*eye)[0],(*eye)[l],(*eye)[2j,(*target)[0],(*target)[l],(*target)[2], 
(int) (twist * 572.957795131)); 
displaythisobject(lightobj), 

} 


void view(quatemion q, vector3D new eye, int viewaxis) 

{ 

Matrix rt = { 1.0,0.0,0 0,0.0, 

0.0, 1.0, 0.0, 0.0, 

0.0, 0.0, 1.0, 0.0, 

0 . 0 , 0 . 0,0 0 , 1 . 0 }; 

matrix3x3 rotation, axis; 
swapbuffers(); 

czclear(0xFFFF7200,getgdesc(GD_ZMAX)); 

loadmatrix(un); 

perspective(450,1.25,0.2,10000.0); 
switch(viewaxis) 

{ 

//Negative Y axis 
case -2: 

axis.DCM_x_rotation( 1.5707963268); 
break; 

//Negative X axis 
case -1: 

axis.DCM_y_rotation(-l .5707963268); 
break; 

//Positive X axis 
case 1: 

axis.DCM_y_rotation( 1.5707963268); 
break; 
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//Positive Y axis 
case 2: 

axis. DCM_x_rotation(-1.5707963268); 
break, 

//Positive Z axis 
case 3: 

axis. DC M_y_rotation(3.14159265359); 
break; 


} 


default: 

break; 


rotation.DCM_body_to_world(q); 
rotation = rotation * axis; 
new eye = new_eye * -1; 


rt[0][0] = rotation[0]; rt[l][0] = rotation[3]; rt[2][0] = 
rt[0][l] = rotation[l]; rt[l][l] = rotation[4]; rt[2][l] = 
rt[0][2] = rotation[2]; rt[l][2] = rotation[5]; rt[2][2] = 
rt[3][0] = 0; rt[3][l] = 0; rt[3][2] = 0; rt[3][3] = 

multmatrix(rt); 


rt[0][0] = 1; rt[l][0] = 0; 

rt[0][l] = 0; rt[l][l] = 1; 
rt[0][2] = 0; rt[l][2] = 0; 

rt[3][0] = new_eye[0], 
rt[3][3] = 1.0. 


rt[2][0] = 0; rt[3][0] = 

rt[2][l] = 0; rt[3][l] = 

rt[2][2] = l; rt[3][2] = 

rt [3][l] = new_eye[l]; 
multmatrix(rt); 


displaythisobject(lightobj); 

} 


void view_axis() 

{ 

displaythisobject(axis); 

} 


void rotate_view(int angle) 

{ 


rotation[6]; rt[3][0] = 0 0; 
rotation[7]; rt[3][l] = 0.0, 
rotation[8]; rt[3][2] = 0 0; 
10 ; 


0 ; 

0 ; 

0; 

rt [3][2] = new_eye[2); 


61 






twist = angle. 


} 

#endif 
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APPENDIX C. RIGID BODY CODE 


A. HEADER FILE 

#ifndef RIGDDBODYH 
#define RIGEDBODYH 
#include <math.h> 
^include "vector3D.H" 
^include "quaternion.H" 
^include "graphics.H" 
^include "time.H" 

#include "matrix3x3.H" 
#include "constants.H" 

class rigid body 


double mass; 
vector3D *location; 
vector3D velocity; 
vector3D acceleration; 
vector3D force; 
quaternion orientation; 


vector3D ang velocity; 

II body coordinates 

vector3D ang acceleration; 

// body coordinates 

vector3D moment; 
matrix3x3 inertia; 
vector3D size; 
double surface_area; 

OBJECT * shape; 
int displayaxis; 
int *display_shape; 
int typebody; 
vector3D holder 1; 
vector3D holder2; 
quaternion holder3; 

// body coordinates 


public: 

void compute_inertia(); 

rigidbodyO; 

rigidbody(int); 

rigid_body(char*); 

void assign mass(double); 

void assign_size(double, double, double); 
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void assignsize(double); 

void assign_size(vector3D); 

void assignsurfacearea(double), 

void assign_inertia(double, double, double); 

void assign_inertia(vector3D); 

void assign_orientation(double, double, double, double), 
void assignorientation(quatemion); 
void assign_shape(OBJECT*); 
void assign type(int); 

void assign holderl (double, double, double); 
void assign_holder2(double, double, double); 
void assign_holder3 (double, double, double, double); 
void assign_holderl(vector3D); 
void assign_holder2(vector3D); 
void assign_holder3 (quaternion); 

// Assign values to the items using doubles in world coordinates 
void assign_location(double, double, double); 
void assign_velocity(double, double, double); 
void assign_acceleration(double, double, double); 
void assign_ang_velocity(double, double, double); 
void assign_ang_acceleration(double, double, double); 
void assign_force(double, double, double); 
void assign_moment(double, double, double); 

// Assign values to the items using vector3D in world coordinates 

void assign location(vector3D); 

void assign_velocity(vector3D); 

void assign_acceleration(vector3 D); 

void assign_ang_velocity(vector3D); 

void assign_ang_acceleration(vector3D); 

void assign_force(vector3D); 

void assign_moment(vector3D); 

// Assign values to the items using doubles in body coordinates 
void assign_velocity_bc(double, double, double); 
void assign_acceleration_bc(double, double, double); 
void assign_ang_velocity_bc(double, double, double); 
void assign_ang_acceleration_bc(double, double, double); 
void assign_force_bc(double, double, double); 
void assign_moment_bc(double, double, double); 

// Assign values to the items using vector3D in body coordinates 
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void assign_velocity_bc(vector3D); 
void assign_acceleration_bc(vector3D), 
void assign_ang_veiocity_bc(vector3D), 
void assign_ang_acceleration_bc(vector3D), 
void assign_force_bc(vector3D), 
void assign_moment_bc(vector3D); 

// Return values of the items world coordinates 

double retum_mass(); 

vector3D retum_inertia(); 

vector3D retum_size(); 

vector3D retum_location(); 

vector3D* retum_location_ptr(); 

vector3D retumvelocityO, 

vector3D retum_acceleration(); 

quaternion retum orientationQ; 

vector3D retum_ang_velocity(); 

vector3D retumangaccelerationO; 

vector3D retumforceO; 

vector3D retum_moment(); 

double retum_surface area(), 

OBJECT* return shape(); 
int return typeO; 
vector3D retum_holderl(); 
vector3D retum_holder20; 
quaternion retum_holder3(); 

// Return values of the items body coordinates 

vector3D retumvelocitybcQ; 

vector3D retumaccelerationbcO; 

vector3D retumangvelocitybcO; 

vector3D retumangaccelerationbcQ; 

vector3D retumforcebcO; 

vector3D retummomentbcO; 

void displayO; 

void update_state(); 

void update_state_rk40; 

double update_state_rk45(double); 

void addaxisO; 

void removeaxisO; 

void attach eyeO; 

void attach targetO; 

void attached_body_update(rigid_body); 
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void zero(), 

~rigid_body() 

{ W 

delete shape; 
delete location, 

} 

}; 

void set eye(double, double, double); 
void set_target(double, double, double); 

#endif 

B. SOURCE FILE 

#ifndef RIGIDBODYC 
#define RIGID_BODY_C 
^include "rigidbody.H" 

void rigid_body::compute_inertia() 

{ 

switch(typebody) 

{ 

case 1: 

inertia[0] = mass * ((size[l] * size[ 1 ]) + (size[2J * size[2])) /12.0 

inertia[4] = mass * ((size[0] * size[0]) + (size[2] * size[2])) /12.0 

inertia[8] = mass * ((size[0] * size[0]) + (size[l] * sizefl])) /12.0 

break; 


case 2: 

inertia[0j maas * ((size[l] * size[l]) + (size[2] * size[2])) / 5.0; 

inertia[4] = mass * ((size[0] * size[0]) + (size[2] * size[2])) / 5.0, 

mertia[8] = mass * ((size[0] * size[0]) + (size[l] * size[l])) / 5.0; 

break; 


} 


case 3: 

inertia[0] = mass * ((size[l] * size[l]) + (size[2] * size[2])) / 4.0; 
inertia[4] = (mass * size[2] * size[2] / 4.0) + 

(mass * size[0] * size[0] /12.0); 
inertia[8] = (mass * size[l] * size[l] / 4.0) + 

(mass * size[0] * size[0] /12.0); 

break; 
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void rigid_body::assign_type(int t) 

{ 

typebody = t; 

} 


rigidbody: :rigid_body() 

{ 

location = new vector3D; 
mass = 1000.0; 
size[0] = 1.0; 
size[l] = 1.0; 
size[2] = 1.0; 
surfacearea = 1.0; 
shape = NULL; 
typebody = 0; 
displayaxis = 0; 
displayshape = new int; 
*display_shape = 1; 

} 


rigid_body::rigid body(char* off_file) 

{ 

location = new vector3D; 

mass = 1000.0; 

size[0] = 1.0; 

size[l] = 1.0; 

size[2] = 1.0; 

surfacearea = 1.0; 

shape = read_object(off_file), 

ready_object_for_display(shape); 

typebody = 0; 

displayaxis = 0; 

display shape = new int; 

*display_shape = 1; 

} 


rigid_body::rigid_body(int n) 
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location = new vector3D, 
mass = 1000.0; 
size[0] =1.0; 
size[l] = 1.0; 
size[2] = 1.0; 
surfacearea =1.0; 
switch(n) 

{ 

case 100: 

shape = read_object("frame.off'); 

typebody = 100; 

break; 

case 200: 

shape = read_object("axis.ofF'); 

type_body = 200; 

break; 


case 1: 

shape = read_object("cube.off'); 
mass = 1000.0; 
inertia[0] = 166.7; 
inertia[4] = 166.7; 
inertia[8] = 166.7; 
typebody = 1; 
surfacearea = 3.0; 
break. 


case 2: 

shape = read_object("sphere. off'), 

typebody = 2; 

mass = 1000.0; 

inertia[0] = 100.0; 

inertia[4] = 100.0; 

inertia[8] = 100.0; 

surfacearea =5; 

break; 


case 3: 

shape = read_object("cylender.ofF'); 
typebody = 3; 
mass = 1000.0; 
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inertia[0] = 500.0, 
inertia[4] = 333.3, 
inertia[8] = 333.3, 
surface_area = 2.0; 
break, 

case 15: 

shape = read_object("fl5.ofF'); 
mass = 19076; 
surfacearea = 5.46, 
typebody =15, 
break; 

case 23: 

shape = read_object("rubber_ban.ofF'); 

mass = 100; 

surfacearea =01, 

type body = 23; 

break; 

case 31: 

shape = read_object("ll.off'); 
mass = 100; 
surfacearea =01; 
typebody = 1; 
break; 

case 32: 

shape = read_object("12.ofT); 

mass = 100, 

surface_area =01; 

typebody = 1; 

break; 

case 33: 

shape = read_object("13.off'); 
mass = 100; 
surfacearea = .01; 
typebody = 1; 
break; 

case 34: 

shape = read_object("14.off'). 
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mass = 100, 
surfacearea = 01; 
typebody - 1, 
break; 

case 35: 

shape = read_object("15.off'); 
mass = 100; 
surfacearea =01; 
type_body = 1; 
break; 

case 50: 

shape = read_object("shuttle.ofT); 

typebody = 50; 

mass = 1570.8; 

inertia[0] = 327.2; 

inertia[4] = 392.7; 

inertia[8] = 327.2; 

break; 

case 60: 

shape = read_object("pansat.off'); 

typebody = 60; 

mass = 46.14179; 

inertiafO] = 0.8914372; 

inertia[4] = 4.081316; 

inertia[8] = 4.082231; 

break; 

case 71: 

shape = read_object("sphere_boom.ofF'); 

mass = 1000.0; 

inertia[0] = 401.0; 

inertia[4] = 473.0; 

inertia[8] = 473.0; 

typebody = 71; 

break; 

case 72: 

shape = read_object(”cube_boom.ofF'); 
mass = 1000.0; 
inertia[0] = 166.8; 
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inertia[4] = 238.8, 
inertia[8] = 238 8, 
typebody = 72, 
break, 

case 73: 

shape = read_object("cylender_boom.ofF'), 

mass = 1000.0, 

inertia[0] = 501.0, 

inertia[4] = 406.0; 

inertia[8] = 406.0; 

typebody = 73; 

break, 

case 90: 

shape = read_object("ground.ofF'); 

typebody = 90; 

break; 

case 91: 

shape = read_object("floor.oflf"); 

typebody = 91; 

break. 


default: 

shape = NULL; 
typebody = 0; 
break; 

} 

if (type body) 

ready_object_for_display( shape); 
displayaxis = 0; 
displayshape = new int; 

*display_shape = 1; 

} 


void rigid_body::assign_shape(OBJECT* o) 

{ 

shape = o; 

} 


71 



void rigid body assign_mass(double n) 

{ 

mass = n, 

} 


void rigid_body::assign_surface_area(double n) 

{ 

surfacearea = n, 

} 


void rigid_body::assign_size(double x, double y, double z) 

{ 

size[0] = x; 
size[l] = y; 
size[2] = z; 

} 


void rigid body : assign_size(double x) 

{ 

size[0] = x; 
size[l] = x; 
size[2] = x; 

} 


void rigid body. :assign_size(vector3D v) 

{ “ ‘ 
size[0] = v[0]; 
size[l] = v[l]; 
size[2] = v[2]; 

} 


void rigid_body::assign_location(double x, double y, double z) 

{ 

(*location)[0] = x; 

(*location)[l] = y; 

(*location)[2] = z; 

} 
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void rigidbody assign_velocity(double x, double y, double z) 

{ 

velocity[0] = x, 
velocity[ 1 ] = y; 
velocity[2] = z; 

1 


void rigid body::assign_acceleration(double x, double y, double z) 

{ 

acceleration[0] = x; 
acceleration 1] = y, 
acceleration^] = z; 

} 


void rigid_body::assign_force(double x, double y, double z) 

{ 

force[0] = x; 
force[ 1 ] = y, 
force[2] = z; 

} 


void rigid body:: assign orientation(double x, double y, double z, double w) 

{ 

orientation[0] = x, 
orientation[ 1 ] = y; 
orientation[2] = z, 
orientation[3] = w; 

} 


void rigid_body: :assign_orientation(quatemion q) 

{ 

orientation[0] = q[0]; 
orientation[l] = q[l], 
orientation[2] = q[2]; 
orientation[3] = q[3]; 

} 
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void rigid body: assign inertia(double x, double y, double z) 

{ 

inertia[0] =x; 
inertia[4] =y; 
inertia[8] =z; 

} 


void rigid body . assign_inertia(vector3D v) 

{ 

inertia[0] = v[0]; 
inertia[4] = v[ 1J; 
inertia[8] = v[2]; 

> 


void rigid body: :assign_ang_velocity(double x, double y, double z) 

{ 

vector3D v(x, y, z); 
matrix3x3 rotation; 

rotation.DCM world to body(orientation); 

v = rotation * v, 

ang_velocity[0] = v[0]; 

ang_velocity[ 1] = vf 1 ]; 

ang_velocitv[2] = v[2]; 

} 


void rigid_body::assign_ang_acceleration(double x, double y, double z) 

{ 

vector3D v(x, y. z); 
matrix3x3 rotation; 

rotation.DCM_world_to_body(orientation); 
v = rotation * v; 
ang_acceleradon[0] = v[0], 
ang_acceleration[l] = v[l]; 
ang_acceleration[2] = v[2]; 

} 


void rigid_body::assign_moment(double x. double y, double z) 

{ 

vector3D v(x, y, z); 
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matrix3x3 rotation, 

rotation. DCM_world_to_body(orientation); 
v = rotation * v; 
moment[0] = v[0], 
moment[l] = v[ 1 ]; 
r nent[2] = v[2]; 


void rigid_body::assign_holderl (double x, double y, double z) 

{ 

holder 1[0] = x, 
holder 1 [ 1 ] = y; 
holder 1 [2] = z; 

} 


void rigid body: :assign_holder2(double x, double y, double z) 

{ 

holder2[0] = x; 
holder2[l] = y; 
holder2[2] = z; 

} 


void rigid body:: assign_holder3 (double x, double y, double z, double w) 

{ 

holder3[0] = x; 
holder3[l] = y; 
holder3[2] = z; 
holder3[3] = w; 

} 


void rigid_body::assign_velocity_bc(double x, double y, double z) 

{ 

vector3D v(x, y, z); 
matrix3x3 rotation; 

rotation. DCM_body_to_world(orientation); 

v = rotation * v; 

velocity[0] = v[0]; 

velocity[l] = v[ 1 ]; 

velocity[2] = v[2]; 
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void rigid_body::assign_acceleration_bc(double x, double y, double z) 

{ ” ' .' 

vector3D v(x, y, z), 
matrix3x3 rotation; 

rotation.DCM_body_to_world(orientation); 

v = rotation * v, 

acceleration [0] = v[0]; 

acceleration! 1] = v[ 1 ]; 

acceleration^] = v[2]; 


void rigid_body::assign_force_bc(double x, double y, double z) 

{ 

vector3D v(x, y, z); 
matrix3x3 rotation; 

rotation.DCM_body_to_world(orientation); 

v = rotation * v; 

force[0] = v[0]; 

forcef 1] = v[l]; 

force[2] = v[2]; 

} 


void rigid_body::assign_ang_velocity_bc(double x, double y, double z) 

{ 

ang_velocity[0] = x; 
ang_velocity[l] = y; 
an g_ ve locity[2] = z; 

} 


void rigid_body::assign_ang_acceleration_bc(double x, double y, double z) 

{ 

ang_acceleration[0] = x; 
ang_acceleration[l] = y; 
ang_acceleration!2] = z; 

} 
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void rigidbody : assign_moment_bc(double x, double y, double z) 

{ ’ 

moment[0] = x; 
moment[ 1 ] = y; 
moment[2] = z; 

} 


void rigid_body::assign_location(vector3D v) 

{ 

(*location)[0] = v[0], 

(*location)[l] = v[l]; 

(*location)[2] = v[2]; 

} 


void rigid_body::assign_velocity(vector3D v) 

{ 

velocity[0] = v[0]; 
velocity[l] = v[l]; 
velocity[2] = v[2]; 

} 


void rigid body: :assign_acceleration(vector3D v) 

{ 

acceleration^] = v[0]; 
acceleration[l] = v[l]; 
acceleration^] = v[2]; 

} 


void rigid_body::assign_force(vector3D v) 

{ 

force[0] = v[0], 
force[l] = v[l]; 
force[2] = v[2]; 

} 


void rigid_body::assign_ang_velocity(vector3D v) 

{ 

matrix3x3 rotation; 


77 







rotation DCM_world_to_body(orientation), 

v = rotation * v, 

ang_velocity[0] = v[0]; 

ang_velocity[ 1 ] = v[ 1 ]; 

ang_velocity[2] = v[2]; 

} 


void rigid body: :assign_ang_acceleration(vector3D v) 

{ 

matrix3x3 rotation; 

ro tation. DC M_world_to_body(onentation); 
v = rotation * v, 
angacceleration[0] = v[0], 
ang_acceleration[ 1 ] — v[ 1 ]; 
ang_acceleration[2] = v[2]; 

} 


void rigid_body::assign_moment(vector3D v) 

{ ' ".. 

matrix3x3 rotation; 

rotation.DCM world to body(orientation); 

v = rotation * v; 

moment[0] = v[0]; 

moment[l] = v[l ]; 

moment[2] = v[2]; 

} 


void rigid_body::assign_holderl(vector3D v) 

{ 

holder 1 = v; 

} 


void rigid_body::assign_holder2(vector3D v) 

{ 

holder2 = v; 

} 


void rigid_body ::assign_holder3 (quaternion v) 
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holder3 = v. 


void rigid_body::assign_velocity_bc(vector3D v) 

{ 

matrix3x3 rotation; 

rotation.DCM_body_to_world(orientation); 

v = rotation * v; 

velocity[0] = v[0], 

velocity[l] = v[l]; 

velocity[2] = v[2]; 

} 


void rigid_body::assign_acceleration_bc(vector3D v) 

{ 

matrix3x3 rotation; 

rotation.DCM_body_to_world(orientation); 
v = rotation * v; 
acceleration^] = v[0]; 
acceleration[l] = v[lj; 
acceleration^] = v[2]; 

} 


void rigid_body::assign_force_bc(vector3D v) 

{ 

matrix3x3 rotation; 

rotation.DCM_body_to_world(orientation); 

v = rotation * v; 

velocity [0] = v[0]; 

velocityfl] = v[l]; 

velocity[2] = v[2]; 


void rigid_body::assign_ang_velocity_bc(vector3D v) 

{ 


ang_velocity[0] = v[0]; 
ang_velocity[l] = v[l]; 
ang_velocity[2] = v[2]; 
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} 


void rigid_body::assign_ang_acceleration_bc(vector3D v) 

{ 

ang_acceleration[0] = v[0]; 
ang_acceleration[l] = v[l]; 
ang_acceleration[2] = v[2]; 

} 


void rigid_body::assign_moment_bc(vector3D v) 

{ 

moment[0] = v[0]; 
moment[l] = v[l]; 
moment[2] = v[2]; 

} 


int rigid_body ::retum_type() 

{ 

return typebody; 

} 


double rigid_body::retum_mass() 

{ 

return mass, 

} 


vector3D rigid_body::retum_inertia() 

{ 

vector3D temp; 
temp[0] = inertia[0]; 
temp[l] = inertia[4]; 
temp[2] = inertia[8]; 
return temp; 

} 


double rigid body: :r um surface areaO 

{ 
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return surface area, 

} 


vector3D rigid_body::retum_size() 

{ 

return size; 

} 


vector3D rigidbody: :retum_location(i 

{ 

return ^location, 

} 


vector3D* rigid body: :retumJocation_ptr() 

{ 

return location; 

} 


vector3D rigid body: :retum_velocity() 

{ 

return velocity; 

} 


vector3D rigid body: :retum_acceleration() 

{ 

return acceleration; 

} 


vector3D rigid_body::retum_force() 

{ 

return force; 

} 


quaternion rigid body: :retum_orientation() 

{ 

return orientation, 
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} 


vector3D rigidbody: :retum_ang_velocity() 

{ 

vector3D v; 
matrix3x3 rotation; 

rotation.DCM_body_to_world(orientation), 
v = rotation * ang velocity; 
return v; 

} 


vector3D rigid body: :retum_ang_acceleration() 

{ 

vector3D v; 
matrix3x3 rotation; 

rotation.DCM_body_to_world(orientation); 
v = rotation * ang acceleration; 
return v; 

} 


vector3D rigidbody: :retum_moment() 

{ 

vector3D v; 
matrix3x3 rotation; 

rotation.DCM_body_to_world(orientation); 
v = rotation * moment; 
return v; 

} 


OBJECT* rigjd_body::retum_shape() 

{ 

return shape; 

} 


vector3D rigid_body::retum_holderlO 

{ 

return holder 1; 

} 


82 


vector3D rigidbody: :retum_holder2() 

{ 

return holder2; 

} 


quaternion rigid body: :retum_holder3() 

{ 

return holder3, 

} 


vector3D rigid body: :retum_velocity_bc() 

{ 

vector3D v; 
matrix3x3 rotation; 

rotation.DCM_world_to_body(orientation); 
v = rotation * velocity; 
return v; 

} 


vector3D rigid body: :retum_acceleration_bc() 

{ 

vector3D v; 
matrix3x3 rotation; 

rotation.DCM_world_to_body(orientation); 
v = rotation * acceleration; 
return v; 

} 


vector3D rigid body: :retum_force_bc() 

{ 

vector3D v; 
matrix3x3 rotation; 

rotation.DCM_world_to_body(orientation); 
v = rotation * force; 
return v; 

} 
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vector3D rigidbody: :retum_ang_velocity_bc() 

{ 

return ang velocity; 

> 


vector3D rigid body: :retum_ang_acceleration_bc() 

{ 

return ang_acceleration; 

} 


vector3D rigid body: :retum_moment_bc() 

{ 

return moment; 

} 


void rigid_body: :update state() 

{ 

vector3D gravity(0.0, -9.81, 0.0); 
matrix3x3 rotation, 
double dt = read_delta(); 

if^gravitystatusO) 

{ 

force = force + (gravity * mass); 

) 

if(air_resistance_statusQ) 

{ 

double magnitude; 

vector3D direction = velocity * -1.0; 

direction.normalizeO; 

magnitude = velocity.magnitudeO * velocity. magnitudeO * 0.12 * 
surfacearea; 

force = force + (direction * magnitude); 

} 

acceleration = force / mass; 

velocity = velocity + (acceleration * dt); 

"“location = "“location + (velocity * dt) - (acceleration * (0.5 * dt * dt)); 
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ang_acceleration[0] = moment[0] / inertia[0], 

ang_acceleration[ 1 ] = moment[l] / inertia[4], 

ang_acceleration[2] = moment[2] / inertia[8]; 

ang velocity = angvelocity + (angacceleration * dt), 

orientation. update(ang velocity, dt), 

orientation normalize(); 

force[0] = 0.0, 

force[l] = 0.0; 

force[2] = 0.0; 

moment[0] = 0.0; 

moment[l] = 0 0; 

moment[2] = 0.0; 


void rigid body: :update_state_rk4() 

{ 

double dt = read delta(); 

double hh = dt * .5, h6 = dt / 6; 

vector3D ya = ang velocity, dyma, dyta, yta, dydxa; 

vector3D yv = velocity, dymv, dytv, ytv, dydxv; 

vector3D yl = ""location, dyml, dytl, ytl, dydxl; 

quaternion y = orientation, dym, dyt, yt, dydx; int i; 

vector3D gravity(0.0, -9.81, 0.0); 

matrix3x3 rotation; 

if(gravity_status0) 

{ 

force = force + (gravity * mass); 

} 

if(air_resistance_status()) 

{ 

double magnitude; 

vectors D direction = velocity * -1.0; 

direction. normalize!); 

magnitude = velocity.magnitude^) * velocity, magnitude!) * 0.12 * 
surfacearea; 

force = force + (direction * magnitude); 

} 

acceleration = force / mass; 
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dydxv = acceleration, dydxl = velocity, 

dvdxa[0] = (moment[0] - (angvelocityp ] * ang_velocity[2] * (inertia[8] - 
inertia[4]))) / inertia[0]; 

dydxa[l] = (moment[l] - (ang_velocity[0] * ang_velocity[2] * (inertia[0] - 
inertia[8]))) / inertia[4]; 

dydxa[2] = (moment[2] - (angvelocityp ] * ang_velocity[0] * (inertia[4] - 
inertia[0]))) / inertia[8], 

dydx[0] = -0 5*((orientation[l] * ang_velocity[0]) + (orientation[2] * 
angvelocityp]) + (orientation[3] * angvelocityp])); 
dydx[l] = 0 5*((orientation[0] * ang_velocity[0]) + (orientation[2] * 
angvelocityp]) - (orientation[3] * angvelocityp])); 
dydx[2] = 0 5*((orientation[0] * ang_velocity[l]) + (orientation[3] * 
ang_velocity[0]) - (orientationp] * angvelocityp])), 
dydx[3] = 0 5*((orientation[0] * ang_velocity[2]) + (orientationp] * 
ang_velocityp]) - (orientation[2] * ang_velocity[0])); 


for(i — 0; i < 3; i++) 

{ 

yt[i] = y[i] + hh * dydx[i], 
yta[i] = ya[i] + hh * dydxa[i]; 
ytv[i] = yv[i] + hh * dydxv[i]; 
ytl[i] = yl[i] + hh * dydxl[i]; 

} 

yt[3] = y[3] + hh * dydx[3]; 
dytv = acceleration; dytl = ytv; 

dyt[0] = -0.5*((yt[l] * yta[0]) + (yt[2] * yta[l]) + (yt[3] * yta[2])); 

dyt[ 1 ] = 0.5*((yt[0] * yta[0]) + (yt[2] * yta[2]) - (yt[3] * yta[l])); 

dyt[2] = 0.5*((yt[0] * yta[l]) + (yt[3] * yta[0]) - (yt[l] * yta[2])); 

dyt[3] = 0.5*((yt[0] * yta[2]) + (yt[l] * yta[l]) - (yt[2] * yta[0])); 

dyta[0] = (moment[0] - (yta[l] * yta[2] * (inertia[8] - inertia[4]))) / inertia[0], 

dytap] = (moment[l] - (yta[0] * yta[2j * (inertia[0] - inertia[8]))) / inertia[4]; 

dyta[2] = (moment[2] - (yta[l] * yta[0] * (inertia[4] - inertia[0]))) / inertia[8]. 


for(i = 0; i < 3; i++) 

{ 

yt[i] = y[i] + hh * dyt[i]; 
yta[i] = ya[i] + hh * dyta[i]; 
ytv[i] = yv[i] + hh * dytv[i]; 
ytl[i] = yl[i] + hh * dvtl[i], 

} 

yt[3] = y[3] + hh * dyt[3]; 
dymv = acceleration; 
dymJ = ytv; 
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dym[0] = -0 5*((yt[l] * yta[0]) + yyt[2] * yta[l]) - (vt[3] * yta[2])); 
dym[l] = 0 5*((yt[0] * yta[0]) + (yt[2] * yta[2]) - (vt[3] * yta[l])), 

dym[2] = 0 5*((yt[0] * yta[l]) + (yt[3] * yta[0]) - (yt[l] * yta[2])); 

dym[3] = 0.5*((yt[0] * yta[2]) + (yt[l] * yta[l]) - (yt[2] * yta[0])); 

dyma[0] - (moment[0] - (yta[l] * yta[2] * (inertia[8] - inertia[4]))) / 

inertia[0], 

dyma[l] = (moment[l] - (yta[0] * yta[2] * (inertia[0] - inertia[8]))) / 
inertia[4], 

dyma[2] = (moment[2] - (yta[l] * yta[0] * (inertia[4] - inertia[0]))) / 
inertia[8], 


for(i — 0; i < 3; i++) 

{ 

yt[i] = y[i] + dt * dym[i]; 
yta[i] = ya[i] + dt * dyma[i]; 
ytv[i] = yv[i] + dt * dymv[i]; 
ytl[i] = yl[i] + dt * dyml[i]; 

} 

yt[3] = y[3] + dt * dym[3]; 

for(i = 0, i < 3; i++) 

{ 

dym[i] = dym[i] + dyt[i], 
dyma[i] - dyma[i] + dyta[i]; 
dymv[i] = dymvfi] + dytv[i]; 
dyml[i] = dyml[i] + dytl[i]; 

} 

dym[3] = dym[3] + dyt[3]; 
dytv = acceleration; 
dytl = ytv; 

dyt[0] = -0.5*((yt[l] * yta[0]) + (yt[2] * yta[l]) + (yt[3j * yta[2])); 

dyt[l] = 0.5*((yt[0j * yta[0]) + (yt[2] * yta[2]) - (yt[3] * yta[l])); 

dyt[2] = 0.5*((yt[0] * yta[l]) + (yt[3] * yta[0]) - (yt[l] * yta[2])); 

dyt[3] = 0.5*((yt[0] * yta[2j) + (yt[l] * yta[l]> - (yt[2] * yta[0])>; 

dyta[0] = (moment[0] - (yta[l] * yta[2] * (inertia[8] - inertia[4]))) / inertia[0]; 

dytaf 1 ] = (moment[ 1 j - (yta[0] * yta[2] * (inertia[0] - inertia[8J))) / inertia[4]; 

dyta[2] = (moment[2] - (ytafl] * yta[0] * (inertia[4] - inertia[0]))) / inertia[8]; 


for(i = 0; i < 3; i++) 

{ 

orientation[i] = y[i] + h6 * (dydx[i] + dyt[i] + 2.0 * dym[i]); 
ang_velocity[i] = ya[i] + h6 * (dydxafi] + dyta[i] + 2.0 * dyma[i]); 
(*location)[i] = yl[i] + h6 * (dydxl[i] + dytl[i] + 2.0 * dyml[i]); 
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velocity[i] = yv[i] + h6 * (dydxv[i] + dytv[i] + 2.0 * dymv[i]); 

I 

orientation[3] = y[3] + h6 * (dydx[3j + dyt[3] + 2.0 * dym[3]), 

orientation. normalize(), 

force[0] = 0.0; 

force[l] = 0 0; 

force[2] = 0.0; 

moment[0] = 0.0; 

moment[l] = 0.0; 

moment[2] = 0.0; 


double rigid_body::update_state_rk4 5 (double eps) 

{ 

double PRGOW = -0.20, PSHRNK = -0.25, FCOR = .06666666, 
dt = readdeltaO; 

double SAFETY = 0.9, ERRCON = 6.0e-4, xsav = dt, htry = dt; 
double P = ang_velocity[0], Q = ang_velocity[ 1 ], R = ang_velocity[2]; 
double hh = dt * .5, h6 = dt / 6, h, dtl, hhl, h61, emnax; 
vector3D ya = angvelocity, dyma, dyta, yta, dydxa, dysava, ysava, ytempa, 
ytemp2a; 

vector3D yv = velocity, dymv, dytv, ytv, dydxv, dysaw, ysaw, ytempv, ytemp2v; 
vector3D yl = *location, dyml, dytl, ytl, dydxl, dysavl, ysavl, ytempl, ytemp21; 
quaternion y = orientation, dym, dyt, yt, dydx, dysav, ysav, ytemp, ytemp2; int i; 
vector3D gravity(0.0, -9.81, 0.0); 
matrix3x3 rotation; 

if(gravity_status()) 

{ 

force = force + (gravity * mass); 

} 

if(air_resistance_statusO) 

{ 

double magnitude; 

vector3D direction = velocity * -1.0; 

direction. normalizeO; 

magnitude = velocity, magnitude!) * velocity, magnitude!) * 0.12 * 
surfacearea; 

force = force + (direction * magnitude); 

} 

acceleration = force / mass; 
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for(i = 0; i < 3, i++) 


ysav[i] = y[i], 
dysavfi] = dydxfi], 
ysavafi] = ya[i], 
dysavafi] = dydxafi], 
vsaw[i] ~ yv[i], 
dysaw[i] = dydxvfi], 
vsavl[i] = ylfi], 
dysavlfi] = dydxJfi], 

j 

ysav[3] = y[3], 
dvsav[3] = dydx[3], 
h = htrv. 
for(,,) 

j 

hh = 0 5 * h. 
dtl = hh, 
hhl = dtl *0 5, 
h61 = dtl / 6 0, 
dydxv = acceleration; 
dydxl = velocity; 

dydxa[0] = (moment[0] - (angvelocityfl] * ang_velocity[2] * (inertia[8] 
inertia[4]))) / inertia[0]; 

dydxa[l] = (moment[l] - (ang_velocity[0] * ang_velocity[2] * (inertia[0] 
inertia[8]))) / inertia[4]; 

dydxa[2] = (moment[2] - (ang_velocity[l] * ang_velocity[0] * (inertia[4] 
inertia[0]))) / inertia[8]; 

dydx[0] = -0.5*((orientation[l] * ang_velocity[0]) + (orientation[2] * 
ang_velocity[l]) + (orientation[3] * ang_velocity[2])); 
dydx[l] = 0.5*((orientation[0] * ang_velocity[0]) + (orientation[2] * 
ang_velocity[2]) - (orientation[3] * ang_velocity[l]»; 
dydx[2] = 0.5*((orientation[0j * ang_velocity[l]) + (orientation[3] * 
ang_velocity[0]) - (orientation[l] * ang_velocity[2])); 
dydx[3] = 0.5*((orientation[0] * ang_velocity[2]) + (orientation[l] * 
ang_velocity[l]) - (orientation[2] * ang_velocity[0])); 


for(i - 0; i < 3; i++) 

{ 

yt[i] = ysav[i] + hhl * dydx[i]; 
yta[i] = ysavafi] + hhl * dydxafi], 
ytvfi] = ysawfi] + hhl * dydxvfi]; 
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vt![i] = ysavlfi] + hhl * dydxlfi], 

} 

yt{3] = ysav{3] hh 1 * dydx[3]. 
dvtv = acceleration, 
dvtl - vtv. 

dyl(0] = -0 5*((yt{ 1 ] * ytafO]) + (vt[2) • vta[ 1 ]) - (yt[3] * yta(2])). 
d\i( 1) = 0 5*((yt[0] * vta[0]) - <yt(2] * yta[2]) - <yt[3] * vta[ 1 ])). 

dvt[2] ' n 5*<(vt{0] * vtaf I]) * (vt(3] * vtafO]) - (yi[ 1 ] * yta(2])). 

d\i[3] - 0 5*<(yt[0] * vta(2)) ♦ (vi( 11 * vtaf I)) - (yi[2] * ytafO])). 

d\ia|0] - (momentfO] - (yta|l) * yta(2] * (inertia(8] - inertia(4]))) 

inertiajO]. 

dvtaf 1 ] - (moment! 1] - <vta(0) * vta(2] * (tnertia{0) - inertiaf8]))) 

inertia(4). 

dvtaf2] = (moment!2) - (yta{ 1 ] * yta[0] * (inertia(4] - inertia{0]))) / 

inertia! 8). 


fort i = 0. i v 3, i-*-*) 

I 

I 

yt(i] = ysav{i] * hhl * dyt|i], 
vtafi] = ysavafi] ^ hhl * dyta[i], 
ytv[i] = ysaw[i] + hhl * dytv[i], 
ytl[i] = ysavlfi] + hhl * dytlfi], 

I 

vt[3] = ysav(3] + hhl * dyt(3], 
dymv = acceleration, 
dyml = ytv, 

dymfO] = -0 5*((yt[l] * ytafO]) + (yt[2] * ytafl]) + (yt[3] * yta[2])); 
dymfl] = 0.5*((yt[0] * ytafOJ) + (yt[2] * yta[2]) - (yt[3] * ytafl])); 

dym[2] = 0.5*((yt[0] * ytafl]) + (yt[3] * ytafO]) - (ytfl] * yta[2])); 

dym[3] = 0.5*((yt[0] * yta[2]) + (ytfl] * ytafl]) - (yt[2] * ytafO])); 

dymafO] = (momentfO] - (ytafl] * yta[2] * (inertia[8] - inertia[4]))) / 

inertiafO]; 

dymafl] = (momentfl] - (ytafO] * yta[2] * (inertiafO] - inertia[8]))) / 
inertia[4]; 

dyma[2] = (momentf2] - (ytafl] * ytafO] * (inertia[4] - inertiafO]))) / 
inertia[8]; 


for(i = 0; i < 3; i++) 

{ 

ytfi] = ysavfi] + dtl * dymfi]; 
ytafi] = ysavafi] + dtl * dytnafi]; 
ytvfi] = ysawfi] + dtl * dymvfi]; 
ytlfi] = ysavlfi] + dtl * dymlfi]; 
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} 

vt[3] = ’ f3] ■+ dtl * dym[3j, 

ford - v i~*) 

! 

dymfi] = dymfi] + dytfi], 
dymafi] = dyma[i] + dyta(i], 
dymvfi] = dymvfi] ■+• dytv[i], 
dymlfi] = dymlfi] - dytlfi], 

j 

dvm[3J = dym(3]-*■ dyt[3], 
dvtv = acceleration, 
dvll = vtv. 

dvt[0] = -0 5*((yt[i] * ytafO]' f2] * vta[l]) + (vt[3] * vta[2])), 
dvt[ IJ = 0 5*((yt{0] * ytafO] J * yta[2]) - (yt[3] • yta[l])), 

dvt[2] = 0 5*((yt[0] • yta[l]) - < t ,] • yU[0J) - (yt[ 1 ] * yta[2])), 

dvt[3] = 0 5*((vt[0] * yta[2]) + (yt[l] ‘ yta[l]) - (yt[2] * yta[0]». 

dvta[0] = (momentfO] - (yta[l] * yta[2] * (inertia[8] - inertia[4]))) / 

inertiafO], 

dyta[ 1 ] = (moment[ 1 ] - (ytafO] * yta[2] * (inert afO] - inerti^8]))) / 

inertia[4], 

dyta[2] = (moment[2] - (yta[ 1] * ytafO] * (inertia[4] - inertiafO])/* / 

inertia[8]. 


for(i = 0, i < 3, i++) 

{ 

ytempfi] = ysav[i] + h61 * (dydx[i] + dyt[i] + 2 0 * dymfi]); 
ytempa[i] = ysava[i] + h61 * (dydxa[i] + dyta[i] + 2.0 * dyma[i]), 
ytemplfi] = ysavl[i] + h61 * (dydxlfi] + dytlfi] + 2 0 * dyml[i]), 
ytempv[i] = ysaw[i] + h61 * (dydxvfi] + dytv[i] + 2.0 * dymv[i]), 

} 

ytemp[3] = vsav[3] + h61 * (dydx[3] + dyt[3] + 2.0 * dym[3]); 

//assign new y's for second rk4 
for(i = 0; i < 3; i++) 

{ 

y[i] = ytempfi]; 
yafi] = ytempafi]; 
ylfi] = ytemplfi]; 
yvfi] = ytempvfi]; 

} 

y[3] = ytemp[3]; 
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//calculate new set of derivatives 

dydxv = acceleration; 

dvd.xl = ytempv; 

dydx[0] = -0 5*((ytemp[l] * ytempa[0]) + (ytemp[2] * ytempa[l]) + 
(ytemp[3] * ytempa[2])); 

dydxfl] = 0.5*((ytemp[0] * ytempa[0]) + (ytemp[2] * ytempa[2]) - 

(ytemp[3] * ytempa[l])); 

dydx[2] = 0 5*((ytemp[0] * ytempap]) + (ytemp[3] * ytempa[0]) - 

(ytemp[l] * ytempa[2])); 

dydx[3] = 0.5*((ytemp[0] * ytempa[2]) + (ytemp[l] * ytempa[l]) - 

(ytemp[2] * ytempa[0])); 

dydxa[0] = (moment[0] - (ytempa[l] * ytempa[2] * (inertia[8] - 
inertia[4]))) / inertia[0]; 

dydxa[ 1 ] = (momentp] - (ytempa[0] * ytempa[2] * (inertia[0] - 
inertia[8]))) / inertia[4], 

dydxa[2] = (moment(2] - (ytempap] * ytempa[0] * (inertia[4] - 
inertia[0]))) / inertia[8]. 


for(i = 0, i < 3, i++) 

{ 

yt[i] = y[i] + hhl * dydx[i], 
yta[i] = ya[i] + hhl * dydxa[i], 
ytv[i] = yv(i] + hhl * dydxv[i], 
ytl[i] = yl[i] + hhl * dydxl[i], 

} 

yt[3] = y[3] + hhl * dydx[3], 
dytv = acceleration, 
dytl = ytv, 

dyt[0] = -0.5*((yt[l] * yta[0]) + (yt[2] * yta[l]) + (yt[3] * yta[2])), 
dyt[ 1 ] = o 5*((yt[0] * yta[0]) + (yt[2] * yta[2]) - (yt[3] * yta[ 1 ])); 

dyt[2] = 0 5*((yt[0] * yta[l J) + (yt[3] * yta[0]) - (yt[l] * yta[2J)), 

dyt[3] = 0.5*((yt[0] * yta[2]) + (yt[l] * yta[l]) - (yt[2] * yta[P])>; 

dyta[0] = (moment[0] - (yta[l] * yta[2] * (inertia[8J - inertir^j))) / 

inertia[0]; 

dyta[l] = (moment[ 1 ] - (yta[0] * yta[2) * (inertia[0) - inertia[8]))) / 

inertia[4]; 

dyta[2] = (moment[2] - (yta[l] * yta[0] * (inertiaf4] - inertia[0]))) / 

inertia[8]. 


for(i = 0; i < 3; i++) 

{ 

yt[i] = y[i] + hhl * dyt[i]; 
yta[i] = ya[i] + hhl * dyta[i]; 
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ytv[i] = yv[i] + hhl * dytv[i]; 
ytl[i] = yl[i] + hhl * dvtl[i] ( 

} 

yt[3] = y[3] -v hhl * dyt[3]; 
dymv = acceleration, 
dyml = ytv; 

dym[0] = -0.5*((yt[l] * yta[0]) + (yt[2] * yta[l]) + (yt[3] * yta[2])); 
dym[l] - 0.5*((yt[0] * yta[0]) + (yt[2] * yta[2]) - (yt[3] * yta[l])); 

dym[2] = 0.5*((yt[0] * yta[l]) + (yt[3] * yta[0]) - (yt[l] * yta[2])); 

dym[3] = 0.5*((yt[0] * yta[2]) + (yt[I] * yta[l]) - (yt[2] * yta[0])), 

dyma[0] = (moment[0] - (yta[ 1 ] * yta[2] * (inertia[8] - inertia[4]))) / 

inertia[0], 

dyma[l] = (moment[l] - (yta[0] * yta[2] * (inertia[0] - inertia[8]))) / 
inertia[4], 

dyma[2] = (moment[2] - (yta[ 1 ] * yta[0] * (inertia[4] - inertia[0]))) / 
inertia[8]. 


for(i = 0, i < 3; i++) 

{ 

yt[i] = y[i] + dtl * dym[i], 
yta[i] = ya[i] + dtl * dyma[i], 
ytv[i] = yv[i] + dtl * dymv(i], 
ytl[i] = yl[i] + dtl * dyml[i], 

} 

yt[3] = y[3] + dtl * dym[3]; 


for(i = 0; i < 3; i++) 

{ 

dym[i] = dym[i] + dyt[i]; 
dyma[i] = dyma[i] + dyta[i]; 
dymv[i] = dymv[i] + dytv[i], 
dyml[i] = dyml[i] + dytlfi]; 

} 

dym[3] = dym[3] + dyt[3], 
dytv = acceleration, 
dytl = ytv, 

dyt[0] = -0.5*((yt[l] * yta[OJ) + (yt[2] * yta(l]) + (yt[3] * yta[2])); 
dyt[ 1 ] = 0.5*((yt[0] * yta[0]) + (yt[2] * yta[2]) - (yt[3] * yta[l])), 

dyt[2] = 0 5*((yt[0] * yta[l]) + (yt[3] * yta[0]) - (yt[l] * yta[2])), 

dyt[3] = 0 5*((yt[0] * yta(2]) + (yt[l] * yta[l]) - (yt[2] * yta[0])), 

dyta[0] = (moment[0] - (yta[ 1 ] * yta[2] * (inertia[8] - inertia[4]))) / 

inertia[0]. 
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dytafl] = (moment[l] - (yta[0] * yta[2] * (inertia[0] - inertia[8]))) / 

inertia[4]; 

dyta[2] = (moment[2] - (yta[ 1J * yta[0] * (inertia[4] - inertia[0]))) / 

inertia[8]; 


for(i = 0; i < 3; i++) 

{ 

ytemp2[i] = y[i] + h61 * (dydx[i] + dyt[i] + 2.0 * dym[i]); 
ytemp2a[i] = ya[i] + h61 * (dydxa[i] + dyta[i] + 2.0 * dyma[i]), 
ytemp21[i] = yl[i] + h61 * (dydxl[i] + dytl[i] + 2 0 * dyml[i]); 
ytemp2v[i] = yv[i] + h61 * (dydxv[i] + dytv[i] + 2.0 * dymv[i]), 

} 

ytemp2[3] = y[3] + h61 * (dydx[3] + dyt[3] + 2 0 * dym[3]); 

//third rk4 run 
dtl = h; 

hhl = dtl * 0.5; 
h51 = dtl / 6.0; 
dydxv = acceleration, 
dydxl = velocity; 

dydxa[0] = (momentfO] - (ang_velocity[ 1 ] * ang_velocity[2] * (inertia[8j 

inertia[4])» / inertia[0]; 

dydxa[l] = (moment[l] - (ang_velocity[0] * ang_velocity[2] * (inertia[0] 

inertia[8]))) / inertia[4]; 

dydxa[2] = (moment[2] - (ang_velocity[ 1 ] * ang_velocity[0] * (inertia[4] 

inertia[0]))) / inertia[8], 

dydx[0] = -0.5*((orientation[l] * ang_velocity[0]) + (orientation[2] * 
ang_velocity[ 1 ]) + (orientation[3] * ang_velocity[2])), 
dydx[l] = 0 5*((orientation[0] * ang_velocity[0]) + (orientation[2] * 
ang_velocity(2j) - (orientation[3] * ang_velocity[l])); 
dydx[2] = 0 5*((orientation[0] * ang_velocity[ 1 ]) + (orientation[3] * 
ang_velocity[0]) - (orientation[ 1 ] * ang_velocity[2])); 
dydx[3] = 0 5*((orientation[0) * ang_vdocity[2]) + (orientation[ 1 ] * 
ang_velocity[l]) - (orientation[2] * ang_velocity[0])); 


for(i = 0; i < 3, i++) 

{ 

yt[i] = ysav{i] + hhl * dydx[i]; 
yta[i] = ysava[i] + hhl * dydxa[i]; 
ytv[i] = ysaw[i] + hhl * dydxv[i]; 
ytl[i] = ysavl[i] + hhl * dydxl[i]; 

} 

yt[3] = ysav[3] + hhl * dydx[3]; 
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dvtv = acceleration, 
dytl = ytv, 

dytfO] = -0 5*((yt[l] * yta[0]) + (yt[2] * yta[l]) + (yt[3] * yta[2])), 
dyt[l] = 0 5*((yt[0] * yta[0]) + (yt[2] * yta[2]) - (yt[3] * yta[l])), 

dyt[2] = 0.5*((yt[0] * ytajl]} + (yt[3] * yta[0]) - (yt[l] * yta[2])), 

dyt[3] = 0.5*((yt[0] * yta[2]) + (yt[l] * yta[l]) - (yt[21 * yta[0])); 

dyta[0] = (moment[0] - (yta[l] * yta[2] * (inertia[8] - inertia[4]))) / 

inertia[0]; 

dyta[l] = (moment[l] - (yta[0] * yta[2] * (inertia[0] - inertia[8]))) / 

inertia[4], 

dyta[2] = (moment[2] - (yta[l] * yta[0] * (inertia[4] - inertia[0]))) / 

inertia[8]; 


for(i = 0; i < 3; i++) 

{ 

yt[i] = ysav[i] + hhl * dyt[i]; 
yta[i] = ysava[i] + hhl * dyta[i]; 
yt y [i] = ysaw[i] + hhl * dytv[i], 
ytl[i] = ysavlfi] + hhl * dytl[i]; 

} 

yt[3] = ysav[3] + hhl * dyt[3], 
dymv = acceleration, 
dyml = ytv; 

dym[0] = -0 5*((yt[l] * yta(0]) + (yt[2] * yta[l)) + (yt[3] * yta[2])), 
dym[l] = 0 5*((yt[0] * yta[0]) + (yt[2J * yta[2]) - (yt[3] * yta[l])); 

dym[2] = 0 5*((yt[0] * yta[l]) + (yt[3] * yta[0]) - (yt[ 1 ] * yta[2])), 

dym[3] = 0 5*((yt[0] * yta[2]) + <yt[ 1 ] * yta[l]) - (yt[2] * yta[0])); 

dyma[0] = (moment[0] - (yta[ 1 ] * yta[2] * (inertia[8] - inertia[4]))) / 

inertia[0], 

dyma[l] = (moment[l] - (yta[0] * yta[2] * (inertia[0] - inertia[8]))) / 
inertia[4], 

dyma[2] = (moment[2] - (yta[ 1 ] * yta[0] * (inertia[4] - inertia[0]))) / 
inertia[8]. 


for(i = 0, i < 3, i++) 

{ 

yt[i] = ysav[i] + dtl * dym[i]; 
yta[i] = ysavafi] + dtl * dyma[i]; 
ytv[i] = ysaw[i] + dtl * dymvfi], 
ytl[i] = ysavl[i] + dtl * dyml[i], 

} 

yt[3] = ysav[3] + dtl * dym[3]; 
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for(i = 0, i < 3, i++) 

r 

dym[i] = dym[i] + dyt[i]; 
dyma[i] = dymafi] + dyta[i], 
dymv[i] = dymv[i] + dytvfi]; 
dyml[i] = dyml[i] + dytl[i], 

} 

dym[3] = dym[3] + dyt[3]; 
dytv = acceleration; 
dytl = ytv; 

dyt[0] = -0.5*((yt[l] * yta[0]) + (yt[2] * yta[l]) + (yt[3] * yta[2])); 
dyt[l] = 0.5*((yt[0] * yta[0]) + (yt[2] * yta[2]) - (yt[3] * yta[l])); 
dyt[2] = 0.5*((yt[0] * yta[l]) + (yt[3] * yta[0]) - (yt[l] * yta[2])); 
dyt[3] = 0.5*((yt[0] * yta[2]) + (yt[l] * ytajlj) - (yt[2] * yta[0])); 
dyta[0] = (moment[0] - (yta[ 1 ] * yta[2] * (inertia[8] - inertia[4]))) / 

inertia[0]; 

dyta[l] = (moment[l] - (yta[0] * yta[2] * (inertiafO] - inertia[8]))) / 

inertia[4]; 

dyta[2] = (moment[2] - (yta[ 1] * yta[0] * (inertia[4] - inertia[0]))) / 

inertia[8]; 


for(i = 0; i < 3; i++) 

{ 

ytemp[i] = ysav[i] + h61 * (dydx[i] + dyt[i] + 2.0 * dym[i]); 
ytempa[i] = ysava[i] + h61 * (dydxafi] + dyta[i] + 2.0 * dyma[i]); 
ytempl[i] = ysavl[i] + h61 * (dydxl[i] + dytl[i] + 2.0 * dyml[i]); 
ytempv[i] = ysaw[i] + h61 * (dydxv[i] + dytv[i] + 2.0 * dymv[i]); 

} 

ytemp[3] = ysav[3] + h61 * (dydx[3] + dyt[3] + 2.0 * dym[3]); 

//error determination 
errmax = 0.0; 
for(i = 0; i < 3; i++) 

{ 

ytemp[i] = ytemp2[i] - ytemp[i]; 

if(errmax < fabs(ytemp[i])) 

errmax = fabs(ytemp[i]); 
ytempafi] = ytemp2a[i] - ytempa[i]; 

if(emnax < fabs(ytempa[i])) 

errmax = fabs(ytempa[i]); 
ytempl[i] = ytemp21[i] - ytempl[i]; 
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if( errmax < fabs(ytempl[i])) 

errmax = fabs(ytempl[i]); 
ytempv[i] = ytemp2v[i] - ytempvfi]; 

if(errmax < fabs(ytempv[i])) 

errmax = fabs(ytempv[i]); 

} 

ytemp[3] = ytemp2[3] - ytemp[3]; 

iflerrmax < fabs(ytemp[3])) 

errmax = fabs(ytemp[3]); 
errmax /= eps; 

if(errmax < 1.0) 
break; 

h = SAFETY * h * exp(PSHRNK*log(errmax)); 

} 

for(i = 0; i < 3; i++) 

{ 

orientation[i] = ytemp2[i] + ytemp[i] * FCOR; 
ang_velocity[i] = ytemp2a[i] + ytempa[i] * FCOR; 
(*location)[i] = ytemp21[i] + ytempl[i] * FCOR; 
velocity[i] = ytemp2v[i] + ytempv[i] * FCOR; 

} 

orientation[3] = ytemp2[3] + ytemp[3] * FCOR; 

orientation.normalizeO; 

force[0] = 0.0; 
force[l] = 0.0; 
force[2] = 0.0; 
moment[0] = 0.0; 
moment[l] = 0.0; 
moment[2] = 0.0; 
return h; 


void rigid body .displayO 

{ 

Matrix rt = { 1.0,0.0,0.0,0 0, 
0 . 0 , 1 . 0 , 0 . 0 , 0 . 0 , 
0 . 0 , 0 . 0 , 1 . 0 , 0 . 0 , 
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0 . 0 , 0 0 , 0 0 , 1 . 0 }; 


Matrix scale = { 10, 0,0, 0 0, 0 0, 
0 0 , 1 . 0 , 0 0 , 0 . 0 , 
0 . 0,0 0 , 1 . 0 , 0 . 0 , 
0 . 0 , 0 . 0 , 0 . 0 , 1 . 0 }; 


pushmatrix(); 

rt[0][0] = ((orientation[0] * orientation[0]) + (orientation} 1] * orientation} 1 ]) - 

(orientation}2] * orientation[2]) - (orientation[3] * orientation}3])); 
rt[l]}0] = 2.0 * ((orientation} 1 ] * orientation}2]) - (orientation}0] * 
orientation[3])); rt[2][0] = 2.0 * ((orientation[0] * orientation}2]) + (orientation} 1 ] 

* orientation[3])); 

rt[0][l] = 2 0 * ((orientation} 1] * orientation[2]) + (orientation[0] * 

orientation[3])); 

rt[l][l] = ((orientation}0] * orientation[0]) - (orientation} 1 ] * orientation} 1]) + 

(orientation[2] * orientation}2]) - (orientation}3] * orientation}3])); 
rt[2][l] = 2.0 * ((orientation[2J * orientation[3]) - (orientation[0] * 
orientation} 1 ])); 

rt[0][2] = 2.0 * ((orientation} 1] * orientation}3j) - (orientation[0] * 
orientation}2])); 

rt[l]}2] = 2.0 * ((orientation[2] * orientation[3]) + (orientationfO] * 

orientation} 1])); 

rt}2]}2] = ((orientation[0] * orientation}0]) - (orientation} 1] * orientation} 1]) - 

(orientation[2] * orientation[2]) + (orientation[3] * orientation}3])); 
rt[3][0] = (*location)[0]; 
rt[3][l] = (*location)[l]; 
rt[3][2] = (*location)[2]; 
multmatrix(rt); 
scale[0][0] = size[0]; 
scale} 1]}1] = slze[l]; 
scale[2][2] = size[2]; 
multmatrix(scale); 

if(display_axis && *display_shape) 

{ 

viewaxisO; 

} 

if(*display_shape) 

{ 

displaythisobject(shape); 

} 
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popmatrix(), 

} 


void rigid_body::add_axis() 

{ 

display axis = 1; 


void rigid body : remove_axis() 

{ 

displayaxis = 0; 

} 


void rigid_body::attach_eye() 

{ 

attach_eye_to(location, displayshape); 

} 


void rigid body: :attach_target() 

{ 

attachtargetto(location); 

} 


void set_eye(double x, double y, double z) 

{ 

set_eye_to(x, y, z); 

} 


void set_target(double x, double y, double z) 

{ 

set_target_to(x, y, z); 

} 


void rigid_body::zero() 

{ 

vector3D zerovector; 
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size[0] =10; 
size[l] = 10; 
size[2] = 1.0; 

♦location = zerovector; 
velocity = zerovector; 
acceleration = zerovector; 
orientation[0] =1.0; 
orientation[l] = 0.0; 
orientation[2] = 0.0; 
orientation[3] = 0.0; 
angveiocity = zerovector; 
angacceleration = zerovector; 


void rigid_body::attached_body_update(rigid_body r) 

{ 

vector3D av; 
matrix3x3 rotation; 

♦location = holder 1; 
update_state(); 
holderl = *location; 

rotation.DCM_body_to_world(r.orientation); 
♦location = (rotation ♦ (*location)) + *r.location; 
holder2 = (rotation * r.ang velocitv) + r.holder2; 
av = holder2; 

rotation.DCM_world_to_body(orientation); 
av = rotation ♦ av; 

orientation.update(av,read_deltaO); 

} 

#endif 


100 


APPENDIX D. VECTOR3D CODE 


A. HEADER FILE 

#ifndef VECTOR3DH 
#define VECTOR3D_H 
#include <iostream.h> 

^include <math.h> 

class vector3D 

i 

l 

double x; 
double y; 
double z; public: 
vector3D(); 

vector3D(double, double, double); 
vector 3 D(const vector3D&); 
void zero(); 

vector3D& operator=(const vector3D&); 
vector3D operator+(const vectoi3D&); 
vector3D operator-(const vector3D&); 

double operator*(const vector3D&); //dot product 

vector3D operator*(double), //scalar multiplication 

vector3D operator/(double); //scalar division 

vector3D operator A (const vector3D&); //cross product 

double magnitudeO; 

void normalizeO, 

void normalize(double); 

friend ostream& operator«(ostream&, vector3D&); 
double& operator[](int); 

~vector3D() { } 

}; 

#endif 

B. SOURCE FILE 

#ifndef VECTOR3DC 
#define VECTOR3D_C 
#include "vector3D.H" 

//default constructor vector3D::vector3DO 

{ 
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x = 0 0, 
y = 0 0. 
z = 0 0. 


//constructor using three doubles 
vector3D vector3D(double a, double b, double c) 
{ 

x = a, 
y = b, 
z = c, 

> 


//constructor using another vector3D 
vector3D: :vector3D(const vector3D& v) 
{ 

x = v.x; 
y = v.y; 
z = v.z; 

} 


//zero out the vector 
void vector3D::zero() 
{ 

x = 0.0; 

y = 0.0; 

z = 0.0; 

} 


//Assignment operator - the function must return a reference to a vector 
//instead of a vector for assignment to work properly 
vector3D& vector3D::operator=(const vector3D& v) 

{ 

x = v.x; 
y = v.y; 
z = v.z; 
return *this; 

} 
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vector addition operator 

vector3D vector3D operator-(const vector3D& \) 

i 

vector3D sum. 
sum x = v x - x. 
sum v = v y - y. 
sum z = v.z -*■ z. 

return sum. 

} 


//vector substraction 

vector3D vector3D::operator-( const vector3D& v) 

{ 

vector3D difF; 
diffx = x - v.x; 
diff.y = y - v.y; 
diff.z = z - v.z; 
return diff; 

} 


//vector dot product 

double vector3D::operator*(const vector3D& v) 

{ 

double dot; 

dot = (v.x * x) + (v.y * y) + (v.z * z); 
return dot; 

} 


//scalar multiplication vector3D vector3D:: operator *(double n) 

{ 

vector3D mult; 
mult.x = x * n; 
mult.y = y * n; 
mult.z = z * n; 
return mult; 

} 
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scalar division - it is the user responsibility to make sure that n is not zero vector3D 
vectoi 3D operator/!double n) 

t 

vector 3D result, 
result x = x / n, 
result y = y / n, 
result z = z / n. 
return result. 


//vector cross product 

vector3D vector3D: :operator A (const vector3D& v) 

{ 

vector3D cross; 
cross.x = (y * v.z) - (v.y * z); 
cross.y = -((x * v.z) - (v.x * z)); 
cross.z = (x * v.y) - (v.x * y); 
return cross; 

} 


//the « operator is to be used with cout 
ostream& operator«(ostream& os, vector3D& v) 

{ 

os « (double) v.x «"," «(double) v.y « "," « (double) v.z « "\n"; 
return os; 

} 


//allows access to the components of the vector3D. it must return a reference 
//in order for assignment to work 
double& vector3D::operator[](int n) 

{ 

if (n = 0) 

{ 

return x; 

} 

if (n — 1) 

{ 

return y; 

} 

if (n = 2) 
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I 

return z. 

> 


//returns the magnitude of the vector 
double vector3D magnitude() 

{ 

return sqrt((x * x) + (y * y) + (z * z)). 


//normalizes the vector to one 
void vector3D::normalize() 

{ 

double m = sqrt((x * x) + (y * y) (z * z)), 
if(m) 

{ 

x = x / m; 


y = y / m; 
z = z / m. 


} 


> 


//normalize the vector to d 

void vector3D normalize(double d) 

{ 

double m = sqrt((x * x) + (y * y) + (z * z)), 
if (m) 

{ 

x = d * x / m, 
y = d * y / m; 
z = d * z / m, 

} 

} 


#endif 
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APPENDIX E. MATRIX3X3 CODE 


A. HEADER FILE 

#ifhdef MATRIX3X3H 
#define MATRIX3X3 H 
#include "vector3D H" 

^include "quatemicn.H" 

#include <iostream h> 

class matrix3x3 

{ 

double m[9]; 

public 

matrix3x3(); 

matrix3x3(double, double, double, double, double, double, double, double, 
double), 

matrix3x3(const matrix3x3&), 
void reset(), 

matrix3x3& operator=(const matrix3x3&); 
matrix3x3 operator+(const matrix3x3&); 
matrix3x3 operator-(const matrix3x3&); 
matrix3x3 operator*(const matrix3x3&); 
void DCM_x_rotation(double); 
void DCM_y_rotation(double); 
void DCM_z_rotation(double); 
void DCM_body_to_world(quatemion); 
void DCM_world_to_body(quatemion); 
void transpose(); 

quaternion generateorientationO; 
vector3D operator* (vector3D&); 
matrix3x3 operator*(double), 
double& operator[](int); 

friend ostream& operator«(ostream&, matrix3x3&); 

~matrix3x3Q { } 

}; 


#endif 


B. SOURCE FILE 

#ifiidef MATRIX3 X3 _C 
#define MATRIX3X3 C 
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#include "matrix3x3 H 


matrix3x3 :matrix3x3() 

{ 

m[0] = 1; 
m[l] = 0; 
m[2] = 0; 
m[3] = 0; 
m[4] = 1, 
m[5] = 0; 
m[6] = 0; 
m[7] = 0, 
m[8] = 1; 

} 


matrix3x3: :matrix3x3(double a, double b, double c, double d, double e, double f, double g, 

double h, double i) 


{ 

m[0] = a; 
m[l] = b; 
m[2] = c; 
m[3] = d; 
m[4] = e; 
m[5] - f; 
m[6] = g; 
m[7] = h; 
m[8] = i; 

} 


matrix3x3::matrix3x3(const matrix3x3& v) 

{ 

m[0] = v.m[0]; 
m[l] = v.m[l]; 
m[2] = v.m[2]; 
m[3] = v.m[3]; 
m[4] = v.m[4]; 
m[5] = v.m[5]; 
m[6] = v.m[6]; 
m[7] = v.m[7]; 
m[8] = v.m[8]; 

} 
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void matrix3x3 : :reset() 

{ 

m[0] = 1; 

m[l] = 0; 
m[2] = 0; 
m[3] = 0; 
m[4] = 1; 
m[5] = 0, 
m[6] = 0; 
m[7] = 0; 
m[8]= 1; 

} 


matrix3x3& matrix3x3 : :operator=(const matrix3x3& v) 

{ 

m[0] = v m[0], 
m[l] = v.m[l], 
m[2] = v.m[2]; 
m[3] = v.m[3], 
m[4] = v.m[4]; 
m[5] = v.m[5]; 
m[6] = v.m[6]; 
m[7] = vm[7); 
m[8] = v m[8]; 
return *this; 


matrix3x3 matrix3x3 ::operator+(const matrix3x3& v) 


matnx3x3 

sum. 


sum.m[0] 

= m[0] 

+ v.m[0], 

sum.m[l] 

= m[l] 

+ v.m[l]; 

summ[2] 

= m[2] 

+ v.m[2]; 

sum.m[3] 

= m[3] 

+ v.m[3]. 

sum.m[4] 

= m[4] 

+ v.m[4]; 

sum.m[5] 

= m[5] 

+ v.m[5]; 

sum.m[6] 

= m[6] 

+ v.m[6]; 

sum.m[7] 

= m[7] 

+ v.m[7]; 

sum.m[8] 

= m[8] 

+ v.m[8]; 
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return sum; 


> 


matrix3x3 matrix3x3operator-( const matrix3x3& v) 

{ 

matrix3x3 difference, 
difference m[0] = m[0] - v.m[0], 
difference m[ 1 ] = m[ 1 ] - v.m[l]; 
difference.m[2] = m[2] - v.m[2], 
difference.m[3] = m[3] - v.m[3], 
difference. m(4] = m[4] - v m[4]; 
difference.m[5] = m[5] - v.mf5], 
difference m{6] = m[6] - v.m[6], 
difference. m[7] = m[7] - v.m[7]; 
difference.m[8] = m[8] - v.m[8], 
return difference; 

} 


matrix3x3 matrix3x3 . operator*(const matrix3x3& v) 

{ 

matrix3x3 temp; 

temp.mfO] = (m[0] * v.m[0]) + (m[l] * v.m[3]) + (m[2] * v.m[6]); 
temp.m[l] = (m[0] * v.m[l]) + (m[l] * v.m[4]) + (m[2] * v.m[7]) ; 
temp m[2] = (m[0] * v.m[2]) + (m[l] * v.m[5]) + (m[2] * v.m[8]); 
temp.m[3] = (m[3] * v.m[0]) + (m[4] * v.m[3]) + (m[5] * v.m[6]); 
temp.m[4] = (m[3] * v.m[l]) + (m[4] * v.m[4]) + (m[5] * v.m[7]); 
temp.m[5] = (m[3] * v.m[2]) + (m{4] * v.m[5]) + (m[5] * v.m[8]) ; 
temp.m[6] = (m[6] * v.m[0]) + (m[7] * v.m[3]> + (m[8] * v.m[6]); 
temp.m[7] = (m[6] * v.m[lj) + (m[7] * v.m[4]) + (m[8] * v.m[7]); 
temp.m[8] = (m[6] * v.m[2]) + (m[7] * v.m[5]) + (m[8] * v.m[8]); 
return temp; 

} 


vector3D matrix3x3: operator*(vector3D& v) 

{ 

vector3D temp = v; 

temp[0] = (m[0] * v[0]) + (m[l] * v[l]) + (m[2] * v[2]); 
temp[ 1] = (m[3] * v[0]) + (m[4] * v[l]> + (m[5] * v[2]); 
temp[2] = (m[6] * v[0]) + (m[7] * v[l]) + (m[8] * v[2]); 
return temp; 
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} 


{ 


} 


matrix3x3 matrix3x3::operator*(double n) 

matrix3x3 product; 
product.m[0] = m[0] * n, 
product.m[l] = m[l] * n, 
product.m[2] = m[2] * n; 
product.m[3] = m[3] * n; 
product.m[4] = m[4] * n, 
product.m[5] = m[5] * n; 
product.m[6] = m[6] * n; 
product.m[7] - m[7] * n, 
product.m[8] = m[8] * n; 
return product; 


ostream& operator«(ostream& os, matrix3x3& v) 

{ 

os «(double) v.m[0] « "," «(double) v.m[l] « ”," «(double)v.m[2] 

«"\n" 

« (double) v.m[3] « "," «(double) v.m[4] « "," « (double) v m[5] 
« "\n" 

«(double) v.m[6] « "," «(double) v.m[7]« "," «(double) v.m[8] 
« "\n" «"\n"; 
return os; 

} 


double& matrix3x3::operator[](int y) 

{ 

return m[y], 

> 


void matrix3x3::DCM_x_rotation(double angle) 

{ 

m(0]= 1.0; 
m[l] = 0.0; 
m[2] = 0.0; 
m[3] = 0.0; 
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m[4] = cos(angle); 
tn[5] = sin(angle); 
m[6] = 0.0; 
m[7] = -sin( angle); 
m[8] - cos(angle), 

> 


void matrix3x3::DCM_y_rotation(double angle) 

{ 

m[0] = cos(angle); 
m[l] = 0.0; 
m[2] = -sin(angle), 
m[3] = 0 0; 
m[4]= 1.0; 
m[5] = 0.0; 
m[6] = sin(angle); 
m[7] = 0 0; 
m[8] = cos(angle); 

} 


void matrix3x3 ::DCM_z_rotation(double angle) 

{ 

m[0] = cos(angle); 
m[l] - sin(angle); 
m[2] = 0.0; 
m[3] = -sin(angle); 
m[4] = cos(angle); 
m[5] = 0.0; 
m[6] = 0.0; 
m[7] = 0.0; 
m[8]= 1.0; 

} 


void matrix3x3::DCM_body_to_world(quatemion orientation) 

{ 

m[0] = ((orientation[0] * orientation[0]) + (orientation[l] * orientation[l]) - 
(orientation[2] * orientation[2]) - (orientation[3] * orientation[3]»; 
m[l] = 2.0 * ((orientation[l] * orientation[2]) - (orientation[0] * orientation[3]»; 
m[2] = 2.0 * ((orientation[0] * orientation[2]) + (orientation[l] * orientation[3])); 
m[3] = 2.0 * ((orientation[ 1] * orientation[2]) + (orientation[0] * orientation[3])); 
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m[4] = ((onentation[0] * orientation[0]) - (orientation! 1] * orientation[l]) + 
(onentation[2] * onentation[2]) - (orientation[3] * orientation[3])), 
m[5] = 20* ((onentation[2] * onentation[3]) - (orientation[0] * orientation[ 1 ])), 
m[6] = 2 0 * ((orientation! 1] * onentation[3]) - (orientation[0] * orientation[2])), 
m[7] = 2 0 * ((orientation[2] * orientation[3]) + (orientation[0] * orientation[l])); 
m[8] = ((orientation[0] * orientation[0]) - (orientation[l] * orientation[l]) - 
(orientation[2] * orientation[2]) + (orientation[3] * orientation^])). 


void matrix3x3 ^CM world_to_body(quaternion orientation) 

I 

m[0] = ((orientation[0] * orientation[0]) + (orientation[l] * orientation[l]) - 
(orientation[2] * orientation[2]) - (orientation[3] * orientation[3])); 
m[3] = 2 0 * ((orientation[ 1 ] * orientation[2]) - (orientation[0] * orientation[3])), 
m[6] = 2.0 * ((orientation[0] * orientation[2]) + (orientation[l] * orientation[3])); 
m[l] = 2.0 * ((orientation[ 1 ] * orientation[2]) + (orientation[0] * orientation[3])); 
m[4] = ((orientation[0] * orientation[0]) - (orientation! 1 ] * orientation! 1]) + 
(orientation[2] * orientation!2]) - (orientation!3] * orientation!3])); 
m!7] = 2.0 * ((orientation[2] * orientation[3j) - (orientationfO] *orientation! 1 ])); 
m[2] = 2.0 * ((orientation! 1] * orientation[3]) - (orientation[o] * orientation!2])); 
m[5] = 2.0 * ((orientation[2] * orientation[3]) + (orientationfO] * orientation! 1])); 
m[8] = ((orientationfO] ■ orientation[0]) - (orientation! 1 ] * orientation! 1]) - 
(orientation[2] * orientation[2]) -r (orientation[3] * 

orientation[3])); 

} 


void matrix3x3: :transpose() 

{ 

matrix3x3 v = *this, 
m[0] = v.mfO]; 
m[l] = v.m[3]; 
m[2] = v.m[6]; 
m[3] = v.m[l]; 
m[4] = v.m[4]; 
m[5] * v.m!7]; 
m[6] = v.m!2]; 
m[7] = v.mf5]; 
m[8] = v.m[8j; 

} 
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quaternion matrix3x3: :generate_orientation() 

{ 

quaternion q,. 

q[0] = 0.5 * sqrt(l + m[0] + m[4] + m[8]); 
q[l j = 0.5 * sqrt(l + m[0] - m[4] - m[8]); 
q[2] = 0.5 * sqrt(l - m[0] + m[4] - m[8]); 
q[3] = 0.5 * sqrt(l - m[0] - m[4] + m[8]); 
q.normalize(); 
return q; 

} 


#endif 
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APPENDIX F. QUATERNION CODE 


A. HEADER FILE 

#ifndef QUATERNIONH 
#define QUATERNIONH 
#include <iostream.h> 
#include <math.h> 

^include "vector3D.H" 

class quaternion 

{ 

double qO; 
double ql; 
double q2; 
double q3; 


public: 

quatemionO; 

quatemion(double, double, double, double); 
quatem ion(const quatemion&); 
void set(double, double, double, double); 
quatemion& operator=(const quatemion&); 
quaternion operator+(const quatemion&); 
quaternion operator-(const quatemion&); 
quaternion operator*(const quatemion&); 
quaternion operator*(double); 
double& operator[](int); 
double magnitudeO; 
void normalizeO; 

quaternion rate_of_change(double, double, double); 

void update(double, double, double, double); 

quaternion rate_ofchange(vector3D); 

void update(vector3D, double); 

friend ostream& operator«(ostream&, quatemion&); 

-quatemionO { } 


#endif 

B. SOURCE FILE 
#ifndef QUATERNIONC 
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#define QUATERNION C 
#include "quaternion.H" 

quaternion: :quatemion() 

{ 

qO = 10; 
ql = 0.0; 
q2 = 0.0; 
q3 - 0.0; 


quaternion.quatemion(double angle_x, double angle_y, double anglez, double rotation) 

{ 

qO = cosf(0.5*rotation); 
q 1 = cosf(angle_x) * sinf(0.5 * rotation); 
q2 = cosf(angle_y)*sinf(0 5*rotation); 
q3 = cosf(angle_z)*sinf(0 5 * rotation); 

} 


quaternion: :quatemion(const quatemion& q) 

{ 

qO = q.qO; 

qi = q qi; 

q2 = qq2; 

q3 = q.q3; 

} 


void quaternion: :set(double angle_x, double angle_y, double angle_z, double rotation) 

{ 

qO = cosf(0.5*rotation); 
ql = cosf(angle_x)*sinf(0.5*rotation); 
q2 = cosf(angle_y) * sinf(0.5 * rotation); 
q3 = cosf(angle_z)*sinf(0.5 *rotation); 

} 


quatemion& quaternion: :operator=(const quatemion& q) 

{ 

qO = q.qO; 

qi = q qi; 
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q2 = q.q2, 
q3 =q.q3; 
return *this, 

} 


quaternion quaternion: :operator+(const quatemion& q) 

{ 

quaternion sum; 
sum.qO = qO + q.qO; 
sum.ql = ql + q.ql; 
sum.q2 = q2 + q.q2; 
sum.q3 = q3 + q.q3; 
return sum; 

} 


quaternion quaternion: :operator-(const quatemion& q) 

{ 

quaternion diff; 
diff.qO = qO - q.qO; 
diff.ql = ql - q.ql; 
diff.q2 = q2 - q.q2; 
diff q3 = q3 - q.q3; 
return diff; 

} 


quaternion quaternion: :operator*(const quatemion& q) 

{ 

quaternion prod; 

prod.qO = (qO * q.qO) - (ql * q.ql) - (q2 * q.q2) - (q3 * q.q3); 

prod.ql = (ql * q.qO) + (qO * q.ql) - (q3 * q.q2) + (q2 * q.q3); 

prod.q2 = (q2 * q.qO) + (q3 * q.ql) - (qO * q.q2) + (ql * q.q3); 

prod.q3 = (q3 * q.qO) + (q2 * q.ql) - (ql * q.q2) + (qO * q.q3); 

return prod; 

} 


quaternion quaternion::operator*(double n) 

{ 

quaternion prod; 
prod.qO = qO * n; 
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} 


prod ql = ql * n; 
prod.q2 = q2 * n, 
prod.q3 = q3 * n; 
return prod; 


double quaternion; :magnitudeO 

{ 

return sqrt((qO * qO)+(ql * ql) + (q2 * q2) + (q3 * q3)); 

} 


void quaternion::nonnalize() 

{ 

double m = sqrt((qO * qO) + (ql * ql) + (q2 * q2) + (q3 * q3)); 
if(m) 

{ 

qO = qO / m; 
ql = ql / m; 
q2 = q2 / m; 
q3 = q3 / m; 

} 

} 


quaternion quaternion; :rate_ofchange(double P, double Q, double R) 

{ 

quaternion rate; 

rate.qO = -0.5*((ql * P) + (q2 * Q) + (q3 * R)); 
rate.ql = 0.5*((q0 * P) + (q2 * R) - (q3 * Q)); 
rate.q2 = 0.5*((q0 * Q) + (q3 * P) - (ql * R)); 
rate.q3 = 0.5*((q0 * R) + (ql * Q) - (q2 * P)); 
return rate; 

} 


void quaternion; ;update(double P, double Q, double R, double sec) 

{ 

double hh = sec * .5, h6 = sec / 6, 
quaternion y = *this, dym, dyt, yt, dydx; 
dydx.qO = -0.5*((ql * P) + ( q 2 * Q) + (q3 * R)); 
dydx.ql = 0.5*((q0 * P) + (q2 * R) - (q3 * Q)); 
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dydx.q2 = 0.5*((q0 * Q) + (q3 * P) - (ql * R)), 

dydx.q3 = 0.5*((q0 * R) + (ql * Q) - (q2 * P)), 

yt.qO = y.qO + hh * dydx.qO; 

yt.ql = y.ql + hh * dydx.ql; 

yt q2 = y.q2 + hh * dydx.q2; 

yt.q3 = y.q3 + hh * dydx.q3; 

dyt.qO = -0.5*((yt.ql * P) + (yt.q2 * Q) + (yt.q3 * R)); 
dyt.ql - 0.5*((yt.q0 * P) + (yt.q2 * R) - (yt.q3 * Q)); 
dyt.q2 = 0.5*((yt.q0 * Q) + (yt.q3 * P) - (yt.ql * R)); 
dyt.q3 = 0.5*((yt.q0 * R) + (yt.ql * Q) - (yt.q2 * P)); 
yt.qO = y.qO + hh * dyt.qO; 
yt.ql = y ql + hh * dyt.ql; 
yt.q2 = y q2 + hh * dyt.q2; 
yt.q3 = y.q3 + hh * dyt.q3; 

dym.qO = -0.5*((yt.ql * P) + (yt.q2 * Q) + (yt.q3 * R)); 

dym.qi = 0.5*((yt.q0 * P) + (yt.q2 * R) - (yt.q3 * Q)); 

dym.q2 = 0.5*((yt.q0 * Q) + (yt.q3 * P) - (yt.ql * R)); 

dym.q3 = 0.5*((yt.q0 * R) + (yt.ql * Q) - (yt.q2 * P»; 

yt.qO = y.qO + sec * dym.qO; 

yt.ql = y.ql + sec * dym.qi; 

yt.q2 = y.q2 + sec * dym.q2; 

yt.q3 = y.q3 + sec * dym.q3; 

dym.qO = dym.qO + dyt.qO; 

dym.qi = dym.qi + dyt.ql; 

dym.q2 = dym.q2 + dyt.q2; 

dym.q3 = dym.q3 + dyt.q3; 

dyt.qO = -0.5*(Gt.ql * P) + (yt.q2 * Q) +• (yt.q3 * R)); 
dyt.ql = 0.5*((yt.q0 * P) + (yt.q2 * R) - (yt.q3 * Q)); 
dyt.q2 = 0.5*((yt.q0 * Q) + (yt.q3 * P) - (yt.ql * R)); 
dyt.q3 = 0.5*((yt.q0 * R) + (yt.ql * Q) - (yt.q2 * P)); 
qO = y.qO + h6 * (dydx.qO + dyt.qO + 2.0 * dym.qO); 
ql = y.ql + h6 * (dydx.ql + dyt.ql + 2.0 * dym.qi); 
q2 = y.q2 + h6 * (dydx.q2 + dyt.q2 + 2.0 * dym.q2); 
q3 = y.q3 + h6 * (dydx.q3 + dyt.q3 + 2.0 * dym.q3); 


quaternion quaternion: :rate_of_change(vector3D ang velocity) 

{ 


quaternion rate; 

rate.qO = -0.5*((ql * ang_velocity[0]) + (q2 * ang_velocity[l]) + (q3 * 
ang_ ve locity[2])); 
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rate ql - 0.5*((q0 * ang_velocity[0]) + (q2 * ang_velocity[2]) - (q3 * 
angvelocityf 1 ])), 

rate.q2 = 0.5*((qO * argvelocityfl]) + (q3 * ang velocityfO]) - (ql * 
ang_velocity[2])); rate.q3 = 0.5*((q0 * ang_velocity[2]) + (ql * 

angvelocityf 1 ]) - (q2 * angvelocityfO])); 
return rate; 

} 


void quaternion: :update(vector3D ang_velocity, double sec) 

double P = ang velocityfO], Q = ang_velocity[l], R = ang_velocity[2]; 

double hh = sec * .5, h6 = sec / 6; 

quaternion y = ♦this, dym, dyt, yt, dydx; 

dydx.qO = -0.5*((ql * P) + ( q 2 * Q) + (q3 * R)); 

dydx.ql = 0.5*((q0 * P) + (q2 * R) - (q3 * Q»; 

dydx.q2 = 0.5*((q0 * Q) + (q3 * P) - (ql * R)); 

dydx.q3 = 0.5*((q0 * R) + (ql * Q) - (q2 * P»; 

yt.qO = y.qO + hh * dydx.qO; 

yt.ql = y.ql + hh * dydx.ql; 

yt.q2 = y.q2 + hh * dydx.q2; 

yt.q3 = y.q3 + hh * dydx.q3; 

dyt qO = -0.5*((yt.ql * P) + (yt.q2 * Q) + (yt. q 3 * R)); 
dyt.ql = 0.5*((yt.q0 * P) + (jt.q2 * R) - (yt.q3 * Q)); 
dyt q2 = 0.5*((yt.q0 * Q) + (yt. q 3 * P) - (yt.ql * R»; 
dyt q3 = 0.5*((yt.q0 * R) + (yt.ql * Q) - (yt. q 2 * P)); 
yt.qO = y.qO + hh * dyt.qO; 
yt.ql = y.ql +hh * dyt.ql; 
yt.q2 = y.q2 + hh * dyt.q2, 
yt q3 = y q3 + hh * dyt.q3; 

dym.qO = -0.5*((yt.ql * P) + (yt.q2 * Q) + (yt.q3 * R»; 

dym.ql = 0.5*((yt.q0 * P) + (yt.q2 * R) - (yt.q3 * Q)) ; 

dym.q2 = 0.5*((yt.q0 * Q) + (yt.q3 * P) - (yt.ql * R)); 

dym.q3 = 0.5*((yt.q0 * R) +(yt.ql * Q) . (yt. q 2 * P»; 

yt.qO = y.qO + sec * dym.qO; 

yt.ql = y.ql + sec * dym.ql; 

yt.q2 = y.q2 + sec * dym.q2; 

yt.q3 = y.q3 + sec * dym.q3; 

dym.qO = dym.qO + dyt.qO; 

dym.ql = dym.ql + dyt.ql; 

dym.q2 = dym.q2 + dyt.q2; 

dym.q3 = dym.q3 + dyt.q3; 

dyt.qO = -0.5*((yt.ql * P) + (yt.q2 * Q) + (yt.q3 * R»; 
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dyt.ql = 0 5*((yt.qO * P) + (yt q2 * R) - (yt q3 * Q)); 
dyt q2 = 0.5*((yt.qO * Q) + (yt q3 * P) - (yt q 1 * R)), 
dyt q3 = 0.5*((yt.q0 * R) + (yt q 1 * Q) - (yt.q2 * P)), 
qO = y qO + h6 * (dydx.qO + dyt qO + 2.0 * dym.qO), 

ql = y.ql + h6 * (dydx ql + dyt ql + 2.0 * dym.ql); 

q2 = y.q2 + h6 * (dydx.q2 + dyt.q2 + 2.0 * dym.q2), 

q3 = y.q3 + h6 * (dydx.q3 + dyt.q3 + 2.0 * dym.q3), 

} 


ostream& operator«(ostream& os, quatemion& q) 

{ 

os «(double) q.qO « "," « (double) q.ql « "," « (double) q.q2 « "," 
« (double) q.q3 « "\n"; 
return os; 

} 


double& quaternion. :operator[](int n) 

{ 

if (n = 0) 

{ 

return qO; 

} 

if(n — 1) 

{ 

return ql; 

} 

if (n — 2) 

{ 

return q2; 

} 

if(n = 3) 

{ 

return q3; 

} 

} 

#endif 
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APPENDIX G. MENU CODE 


A. HEADER FILE 

#ifhdef MENU_H 
#define MENU H 
#include "menu.H" 
#include <gl.h> 

#include <device.h> 
include <stdio.h> 
^include <stdlib.h> 

void initialize_menu(); 
int queue testO; 

#endif 


B. SOURCE FILE 

#ifhdef MENUC 
^define MENU C 
#include "menu.H" 
^include "time.H" 
#include <iostream.h> 

long mainmenu; 
long hititem, 
short value; 
double wx, wy; 


long makethemenusO /* this routine performs all the menu construction calls */ 

long topmenu; 

topmenu = defpup("Dynamics Visualizer %t | Exit %x99"); 
retum(topmenu); 

} 


void initializemenuO 

{ 

mainmenu = makethemenusO; 

} 
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void processmenuhit(long hititem) 

{ 

switch(hititem) 

{ 

case 99 

exit(O) 

break; 

default: 

break; 

} /* end switch */ 

} 


int queue testO 

{ 

hititem = 0; 

while(qtestO) 

{ 

switch(qread(&value)) 

{ 

case MENUBUTTON: 
if( value — 1) 

{ 


mmode(MSINGLE); 
hititem = dopup(mainmenu); 
mmode(MVIEWING), 
processmenuhit(hititem); 
resettimeO; 

} 

break; 


case LEFTMOUSE: 

wx = getvaluator(MOUSEX); 
wy = getvaluator(MOUSEY), 
hititem = (wx * 100000) + wy; 

break; 


case REDRAW: 

reshapeviewportO; 

break; 
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case UPARROWKEY 
hititem = 100, 
break, 

case DOWNARROWKEY 
hititem =101, 
break, 

case LEFTARROWKEY: 
hititem =102, 
break, 

case RIGHTARROWKEY 
hititem =103, 
break; 

case EQUALKEY: 
hititem = 104; 
break, 

case MINUS KEY 

hititem =105; 
break; 

case SPACEKEY: 

hititem = 106; 
break; 

caseFIKEY: 

hititem = 111; 
break; 

case F2KEY: 

hititem =112; 
break; 

case F3KEY: 

hititem = 113; 
break; 

caseF4KEY: 

hititem =114; 
break; 
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case F5KEY 

hititem =115, 
break, 

case F6KEY: 

hititem =116; 
break; 

case F7KEY: 

hititem =117; 
break, 

case F8KEY: 

hititem =118; 
break; 

case F9KEY: 

hititem = 119; 
break, 

case F10KEY: 

hititem = 120; 
break; 

case FI 1KEY. 

hititem = 121; 
break; 

caseF12KEY: 

hititem = 122; 
break; 


default; 

break; 

} 

} 

return (int) hititem; 


#endif 
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APPENDIX H. TIME CODE 


A. HEADER FILE 

#ifndef TIME H 
#define TIME H 
^include <time h> 

#include <sys/types.h> 

#include <sys/times h> 

#include <sys/param.h> 

void set_delta(), 
void set delta(double); 
void set time(): 
void reset_ume(); 
double read delta(), 
double read_time(), 
int read_ticks(), 

void set real time factor(double); 
#endif 


B. SOURCE FILE 

#ifiidef TIMEC 
#defir.e TIMEC 
^include "time.H" 

struct tms timebuffer; 

long old time; double delta, real_time_ratio =1.0; 

void set deltaO 

{ 

delta = ((double) (times(#timebuffer) - old_time)/HZ) * real_time_ratio; 
oldtime = times(&timebufifer); 

} 


void set_delta(double step) 

{ 

delta = step; 

oldtime = times(&timebuffer); 

} 
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void set_time() 

{ 

oldtime = times(&timebuffer); 

} 


double read_delta() 

{ 

return delta; 

} 


double read_time() 

< 

return (double) old time; 

} 


int read_ticks() 

{ 

return (int) times(&timebuffer); 

} 


void set_real_time_factor(double f) 

{ 

realtimeratio = f; 

} 


void reset timeO 

{ 

long deltaticks; 

delta_ticks = (long) (delta * HZ); 
old_time = times(&timebuflfer) - delta_ticks; 

} 

#endif 
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APPENDIX I. ALTERING THE GRAVITY GRADIENT VISUALIZER CODE 


Altering the Gravity Gradient Visualizer code is a tedious process not to be 
attempted unless one has a working knowledge of ‘.he UNIX operating system, computer 
graphics and C++ programming. If the user feels the need to make changes to the code, 
the following steps should be followed: 

• After logging on to a Silicon Graphics computer, at the UNIX prompt type 
"cd -jstewart/thesis". This will put the user in the directory where the code 
resides. 

• Using any text editor, edit the file to be changed, make and save the changes, 
and exit the editor. 

• At the UNIX prompt type "make". This will run a makefile which will determine 
that there is an update to a file and recompile and relink the file 

• Run the program as usual. 

As an example, to change the default click value for the mass field from 50 kilogram 
increments to 10 kilogram increments, log on, change to the proper directory and edit the 
file "ggrad.C". Page down the file until you find a line that reads "mass = mass + 50;" and 
change the "50" to "10". Then find a line that reads "mass = mass - 50;" and change the 
"50" to "10". Save the changes and exit the editor. At the UNIX prompt type "make" to 
recompile and relink the code. Run the program as usual. This process can be repeated 
for any changes one wishes to make. Although this is a straightforward process, there are 
many opportunities for error, and these errors can be difficult to find. Therefore, it can 


129 


not be emphasized enough that this should not be attempted by one without the requisite 
background in the aforementioned areas. 
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